0.4.1
Loading...
Searching...
No Matches
NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType > Class Template Reference

Keyed Kalman Filter class. More...

Data Structures

struct  NISResult
 Normalized Innovation Squared (NIS) test results. More...
 
struct  SavedPreUpdate
 Saved pre-update state and measurement. More...
 

Public Member Functions

void addMeasurement (const MeasKeyType &measKey)
 Add a measurement to the filter.
 
void addMeasurements (std::span< const MeasKeyType > measKeys)
 Add measurements to the filter.
 
void addState (const StateKeyType &stateKey)
 Add a new state to the filter.
 
void addStates (std::span< const StateKeyType > stateKeys)
 Add new states to the filter.
 
void calcPhiAndQWithVanLoanMethod (Scalar dt)
 Numerical Method to calculate the State transition matrix ๐šฝ and System/Process noise covariance matrix ๐
 
void calcTransitionMatrix_Phi_exp (Scalar tau)
 Calculates the state transition matrix ๐šฝ using the exponential matrix.
 
void calcTransitionMatrix_Phi_Taylor (Scalar tau, size_t order)
 Calculates the state transition matrix ๐šฝ limited to specified order in ๐…๐œโ‚›
 
auto checkForOutliersNIS (const std::string &nameId)
 Performs the Normalized Innovation Squared (NIS) test on the measurement innovation ๐œน๐ณ
 
void correct ()
 Do a Measurement Update with a Measurement ๐ณ
 
void correctWithMeasurementInnovation ()
 Do a Measurement Update with a Measurement Innovation ๐œน๐ณ
 
void discardPreUpdate ()
 Discards the saved xฬ‚, ๐, ๐ณ, ๐‡, ๐‘ a-priori (pre-update)
 
bool hasAnyStates (std::span< const StateKeyType > stateKeys) const
 Checks if the filter has any of the provided keys.
 
bool hasState (const StateKeyType &stateKey) const
 Checks if the filter has the key.
 
bool hasStates (std::span< const StateKeyType > stateKeys) const
 Checks if the filter has the keys.
 
bool isNISenabled () const
 Whether the NIS check should be performed.
 
bool isPreUpdateSaved () const
 Whether a pre-update was saved.
 
 KeyedKalmanFilter ()=default
 Default Constructor.
 
 KeyedKalmanFilter (std::span< const StateKeyType > stateKeys, std::span< const MeasKeyType > measKeys)
 Constructor.
 
void predict ()
 Do a Time Update.
 
void removeMeasurement (const MeasKeyType &measKey)
 Remove a measurement from the filter.
 
void removeMeasurements (std::span< const MeasKeyType > measKeys)
 Remove measurements from the filter.
 
void removeState (const StateKeyType &stateKey)
 Remove a state from the filter.
 
void removeStates (std::span< const StateKeyType > stateKeys)
 Remove states from the filter.
 
void replaceState (const StateKeyType &oldKey, const StateKeyType &newKey)
 Replace the old with the new key.
 
void restorePreUpdate ()
 Restores the saved xฬ‚, ๐, ๐ณ, ๐‡, ๐‘ a-priori (pre-update)
 
const SavedPreUpdatesavedPreUpdate () const
 Accesses the saved pre-update matrices.
 
void savePreUpdate ()
 Saves xฬ‚, ๐, ๐ณ, ๐‡, ๐‘ a-priori (pre-update)
 
void setMeasurements (std::span< const MeasKeyType > measKeys)
 Sets the measurement keys and initializes matrices z, H, R, S, K with Zero.
 
void setZero ()
 Sets all Vectors and matrices to 0.
 
bool showKalmanFilterGUI (const char *id, float width=0.0F)
 Shows GUI elements for the Kalman Filter.
 
void showKalmanFilterMatrixViews (const char *id, int nRows=-2)
 Shows ImGui Tree nodes for all matrices.
 

Data Fields

KeyedMatrixX< Scalar, StateKeyType, StateKeyType > F
 ๐… System model matrix (n x n)
 
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > G
 ๐† Noise input matrix (n x o)
 
KeyedMatrixX< Scalar, MeasKeyType, StateKeyType > H
 ๐‡ Measurement sensitivity matrix (m x n)
 
KeyedMatrixX< Scalar, StateKeyType, MeasKeyType > K
 ๐Š Kalman gain matrix (n x m)
 
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > P
 ๐ Error covariance matrix (n x n)
 
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > Phi
 ๐šฝ State transition matrix (n x n)
 
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > Q
 ๐ System/Process noise covariance matrix (n x n)
 
KeyedMatrixX< Scalar, MeasKeyType, MeasKeyType > R
 ๐‘ = ๐ธ{๐ฐโ‚˜๐ฐโ‚˜แต€} Measurement noise covariance matrix (m x m)
 
KeyedMatrixX< Scalar, MeasKeyType, MeasKeyType > S
 ๐—ฆ Measurement prediction covariance matrix (m x m)
 
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > W
 ๐– Noise scale matrix (o x o)
 
KeyedVectorX< Scalar, StateKeyType > x
 xฬ‚ State vector (n x 1)
 
KeyedVectorX< Scalar, MeasKeyType > z
 ๐ณ Measurement vector (m x 1)
 

Private Attributes

double _alphaNIS
 NIS Test Hypothesis testing failure rate (probability that H_0 is accepted is (1 - alpha))
 
bool _checkNIS
 Normalized Innovation Squared (NIS) test.
 
SavedPreUpdate _savedPreUpdate
 Saved pre-update state and measurement.
 
Eigen::MatrixXd I
 ๐‘ฐ Identity matrix (n x n)
 

Friends

void from_json (const json &j, KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType > &obj)
 Converts the provided json object into a node object.
 
void to_json (json &j, const KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType > &obj)
 Converts the provided object into json.
 

Detailed Description

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
class NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >

Keyed Kalman Filter class.

Template Parameters
ScalarNumeric type, e.g. float, double, int or std::complex<float>.
StateKeyTypeType of the key used for state lookup
MeasKeyTypeType of the key used for measurement lookup

Definition at line 38 of file KeyedKalmanFilter.hpp.

Constructor & Destructor Documentation

◆ KeyedKalmanFilter() [1/2]

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::KeyedKalmanFilter ( )
default

Default Constructor.

◆ KeyedKalmanFilter() [2/2]

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::KeyedKalmanFilter ( std::span< const StateKeyType > stateKeys,
std::span< const MeasKeyType > measKeys )
inline

Constructor.

Parameters
stateKeysState keys
measKeysMeasurement keys

Definition at line 47 of file KeyedKalmanFilter.hpp.

Member Function Documentation

◆ addMeasurement()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::addMeasurement ( const MeasKeyType & measKey)
inline

Add a measurement to the filter.

Parameters
measKeyMeasurement key

Definition at line 247 of file KeyedKalmanFilter.hpp.

◆ addMeasurements()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::addMeasurements ( std::span< const MeasKeyType > measKeys)
inline

Add measurements to the filter.

Parameters
measKeysMeasurement keys

Definition at line 251 of file KeyedKalmanFilter.hpp.

◆ addState()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::addState ( const StateKeyType & stateKey)
inline

Add a new state to the filter.

Parameters
stateKeyState key

Definition at line 154 of file KeyedKalmanFilter.hpp.

◆ addStates()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::addStates ( std::span< const StateKeyType > stateKeys)
inline

Add new states to the filter.

Parameters
stateKeysState keys

Definition at line 158 of file KeyedKalmanFilter.hpp.

◆ calcPhiAndQWithVanLoanMethod()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::calcPhiAndQWithVanLoanMethod ( Scalar dt)
inline

Numerical Method to calculate the State transition matrix ๐šฝ and System/Process noise covariance matrix ๐

Parameters
[in]dtTime step in [s]
Note
See C.F. van Loan (1978) - Computing Integrals Involving the Matrix Exponential [48]

Definition at line 322 of file KeyedKalmanFilter.hpp.

◆ calcTransitionMatrix_Phi_exp()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::calcTransitionMatrix_Phi_exp ( Scalar tau)
inline

Calculates the state transition matrix ๐šฝ using the exponential matrix.

Parameters
[in]tauTime interval in [s]
Note
See [17] Groves, ch. 3.2.3, eq. 3.33, p. 97
Attention
The cost of the computation is approximately 20n^3 for matrices of size n. The number 20 depends weakly on the norm of the matrix.

Definition at line 312 of file KeyedKalmanFilter.hpp.

◆ calcTransitionMatrix_Phi_Taylor()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::calcTransitionMatrix_Phi_Taylor ( Scalar tau,
size_t order )
inline

Calculates the state transition matrix ๐šฝ limited to specified order in ๐…๐œโ‚›

Parameters
[in]tauTime interval in [s]
[in]orderThe order of the Taylor polynom to calculate
Note
See [17] Groves, ch. 3.2.3, eq. 3.34, p. 98

Definition at line 301 of file KeyedKalmanFilter.hpp.

◆ checkForOutliersNIS()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
auto NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::checkForOutliersNIS ( const std::string & nameId)
inlinenodiscard

Performs the Normalized Innovation Squared (NIS) test on the measurement innovation ๐œน๐ณ

Parameters
[in]nameIdCaller node for debug output

H_0: The measurement residual ๐œน๐ณ is consistent with the innovation covariance matrix ๐—ฆ The acceptance interval is chosen such that the probability that H_0 is accepted is (1 - alpha)

Returns
The hypothesis test result if it failed, otherwise nothing.
Attention
Needs to be called before the update

Definition at line 402 of file KeyedKalmanFilter.hpp.

◆ correct()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::correct ( )
inline

Do a Measurement Update with a Measurement ๐ณ

Attention
Update the Measurement sensitivity Matrix (๐‡), the Measurement noise covariance matrix (๐‘) and the Measurement vector (๐ณ) before calling this
Note
See P. Groves (2013) - Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (ch. 3.2.2)

Definition at line 106 of file KeyedKalmanFilter.hpp.

◆ correctWithMeasurementInnovation()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::correctWithMeasurementInnovation ( )
inline

Do a Measurement Update with a Measurement Innovation ๐œน๐ณ

Attention
Update the Measurement sensitivity Matrix (๐‡), the Measurement noise covariance matrix (๐‘) and the Measurement vector (๐ณ) before calling this
Note
See P. Groves (2013) - Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (ch. 3.2.2)
See Brown & Hwang (2012) - Introduction to Random Signals and Applied Kalman Filtering (ch. 5.5 - figure 5.5)

Definition at line 125 of file KeyedKalmanFilter.hpp.

◆ discardPreUpdate()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::discardPreUpdate ( )
inline

Discards the saved xฬ‚, ๐, ๐ณ, ๐‡, ๐‘ a-priori (pre-update)

Definition at line 378 of file KeyedKalmanFilter.hpp.

◆ hasAnyStates()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
bool NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::hasAnyStates ( std::span< const StateKeyType > stateKeys) const
inlinenodiscard

Checks if the filter has any of the provided keys.

Parameters
stateKeysState keys

Definition at line 150 of file KeyedKalmanFilter.hpp.

◆ hasState()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
bool NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::hasState ( const StateKeyType & stateKey) const
inlinenodiscard

Checks if the filter has the key.

Parameters
stateKeyState key

Definition at line 144 of file KeyedKalmanFilter.hpp.

◆ hasStates()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
bool NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::hasStates ( std::span< const StateKeyType > stateKeys) const
inlinenodiscard

Checks if the filter has the keys.

Parameters
stateKeysState keys

Definition at line 147 of file KeyedKalmanFilter.hpp.

◆ isNISenabled()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
bool NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::isNISenabled ( ) const
inlinenodiscard

Whether the NIS check should be performed.

Definition at line 385 of file KeyedKalmanFilter.hpp.

◆ isPreUpdateSaved()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
bool NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::isPreUpdateSaved ( ) const
inlinenodiscard

Whether a pre-update was saved.

Definition at line 334 of file KeyedKalmanFilter.hpp.

◆ predict()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::predict ( )
inline

Do a Time Update.

Attention
Update the State transition matrix (๐šฝ) and the Process noise covariance matrix (๐) before calling this
Note
See P. Groves (2013) - Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (ch. 3.2.2)

Definition at line 93 of file KeyedKalmanFilter.hpp.

◆ removeMeasurement()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::removeMeasurement ( const MeasKeyType & measKey)
inline

Remove a measurement from the filter.

Parameters
measKeyMeasurement key

Definition at line 266 of file KeyedKalmanFilter.hpp.

◆ removeMeasurements()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::removeMeasurements ( std::span< const MeasKeyType > measKeys)
inline

Remove measurements from the filter.

Parameters
measKeysMeasurement keys

Definition at line 270 of file KeyedKalmanFilter.hpp.

◆ removeState()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::removeState ( const StateKeyType & stateKey)
inline

Remove a state from the filter.

Parameters
stateKeyState key

Definition at line 180 of file KeyedKalmanFilter.hpp.

◆ removeStates()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::removeStates ( std::span< const StateKeyType > stateKeys)
inline

Remove states from the filter.

Parameters
stateKeysState keys

Definition at line 184 of file KeyedKalmanFilter.hpp.

◆ replaceState()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::replaceState ( const StateKeyType & oldKey,
const StateKeyType & newKey )
inline

Replace the old with the new key.

Parameters
[in]oldKeyOld key to replace
[in]newKeyNew key to use instead

Definition at line 207 of file KeyedKalmanFilter.hpp.

◆ restorePreUpdate()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::restorePreUpdate ( )
inline

Restores the saved xฬ‚, ๐, ๐ณ, ๐‡, ๐‘ a-priori (pre-update)

Definition at line 358 of file KeyedKalmanFilter.hpp.

◆ savedPreUpdate()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
const SavedPreUpdate & NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::savedPreUpdate ( ) const
inlinenodiscard

Accesses the saved pre-update matrices.

Definition at line 547 of file KeyedKalmanFilter.hpp.

◆ savePreUpdate()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::savePreUpdate ( )
inline

Saves xฬ‚, ๐, ๐ณ, ๐‡, ๐‘ a-priori (pre-update)

Definition at line 337 of file KeyedKalmanFilter.hpp.

◆ setMeasurements()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::setMeasurements ( std::span< const MeasKeyType > measKeys)
inline

Sets the measurement keys and initializes matrices z, H, R, S, K with Zero.

Parameters
measKeysMeasurement keys

Definition at line 228 of file KeyedKalmanFilter.hpp.

◆ setZero()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::setZero ( )
inline

Sets all Vectors and matrices to 0.

Definition at line 74 of file KeyedKalmanFilter.hpp.

◆ showKalmanFilterGUI()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
bool NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::showKalmanFilterGUI ( const char * id,
float width = 0.0F )
inline

Shows GUI elements for the Kalman Filter.

Parameters
[in]idUnique id for ImGui
[in]widthWidth of the widget
Returns
True if something was changed

Definition at line 432 of file KeyedKalmanFilter.hpp.

◆ showKalmanFilterMatrixViews()

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::showKalmanFilterMatrixViews ( const char * id,
int nRows = -2 )
inline

Shows ImGui Tree nodes for all matrices.

Parameters
[in]idUnique id for ImGui
[in]nRowsAmount of rows to show

Definition at line 457 of file KeyedKalmanFilter.hpp.

Friends And Related Symbol Documentation

◆ from_json

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void from_json ( const json & j,
KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType > & obj )
friend

Converts the provided json object into a node object.

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

Definition at line 573 of file KeyedKalmanFilter.hpp.

◆ to_json

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
void to_json ( json & j,
const KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType > & obj )
friend

Converts the provided object into json.

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

Definition at line 563 of file KeyedKalmanFilter.hpp.

Field Documentation

◆ _alphaNIS

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
double NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::_alphaNIS
private

NIS Test Hypothesis testing failure rate (probability that H_0 is accepted is (1 - alpha))

Definition at line 558 of file KeyedKalmanFilter.hpp.

◆ _checkNIS

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
bool NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::_checkNIS
private

Normalized Innovation Squared (NIS) test.

Definition at line 555 of file KeyedKalmanFilter.hpp.

◆ _savedPreUpdate

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
SavedPreUpdate NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::_savedPreUpdate
private

Saved pre-update state and measurement.

Definition at line 552 of file KeyedKalmanFilter.hpp.

◆ F

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
KeyedMatrixX<Scalar, StateKeyType, StateKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::F

๐… System model matrix (n x n)

Definition at line 293 of file KeyedKalmanFilter.hpp.

◆ G

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
KeyedMatrixX<Scalar, StateKeyType, StateKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::G

๐† Noise input matrix (n x o)

Definition at line 294 of file KeyedKalmanFilter.hpp.

◆ H

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
KeyedMatrixX<Scalar, MeasKeyType, StateKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::H

๐‡ Measurement sensitivity matrix (m x n)

Definition at line 288 of file KeyedKalmanFilter.hpp.

◆ I

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
Eigen::MatrixXd NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::I
private

๐‘ฐ Identity matrix (n x n)

Definition at line 550 of file KeyedKalmanFilter.hpp.

◆ K

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
KeyedMatrixX<Scalar, StateKeyType, MeasKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::K

๐Š Kalman gain matrix (n x m)

Definition at line 291 of file KeyedKalmanFilter.hpp.

◆ P

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
KeyedMatrixX<Scalar, StateKeyType, StateKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::P

๐ Error covariance matrix (n x n)

Definition at line 284 of file KeyedKalmanFilter.hpp.

◆ Phi

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
KeyedMatrixX<Scalar, StateKeyType, StateKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::Phi

๐šฝ State transition matrix (n x n)

Definition at line 285 of file KeyedKalmanFilter.hpp.

◆ Q

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
KeyedMatrixX<Scalar, StateKeyType, StateKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::Q

๐ System/Process noise covariance matrix (n x n)

Definition at line 286 of file KeyedKalmanFilter.hpp.

◆ R

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::R

๐‘ = ๐ธ{๐ฐโ‚˜๐ฐโ‚˜แต€} Measurement noise covariance matrix (m x m)

Definition at line 289 of file KeyedKalmanFilter.hpp.

◆ S

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
KeyedMatrixX<Scalar, MeasKeyType, MeasKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::S

๐—ฆ Measurement prediction covariance matrix (m x m)

Definition at line 290 of file KeyedKalmanFilter.hpp.

◆ W

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
KeyedMatrixX<Scalar, StateKeyType, StateKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::W

๐– Noise scale matrix (o x o)

Definition at line 295 of file KeyedKalmanFilter.hpp.

◆ x

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
KeyedVectorX<Scalar, StateKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::x

xฬ‚ State vector (n x 1)

Definition at line 283 of file KeyedKalmanFilter.hpp.

◆ z

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
KeyedVectorX<Scalar, MeasKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::z

๐ณ Measurement vector (m x 1)

Definition at line 287 of file KeyedKalmanFilter.hpp.


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