![]() |
0.4.1
|
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 SavedPreUpdate & | savedPreUpdate () 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. | |
Keyed Kalman Filter class.
Scalar | Numeric type, e.g. float, double, int or std::complex<float>. |
StateKeyType | Type of the key used for state lookup |
MeasKeyType | Type of the key used for measurement lookup |
Definition at line 38 of file KeyedKalmanFilter.hpp.
|
default |
Default Constructor.
|
inline |
Constructor.
stateKeys | State keys |
measKeys | Measurement keys |
Definition at line 47 of file KeyedKalmanFilter.hpp.
|
inline |
Add a measurement to the filter.
measKey | Measurement key |
Definition at line 247 of file KeyedKalmanFilter.hpp.
|
inline |
Add measurements to the filter.
measKeys | Measurement keys |
Definition at line 251 of file KeyedKalmanFilter.hpp.
|
inline |
Add a new state to the filter.
stateKey | State key |
Definition at line 154 of file KeyedKalmanFilter.hpp.
|
inline |
Add new states to the filter.
stateKeys | State keys |
Definition at line 158 of file KeyedKalmanFilter.hpp.
|
inline |
Numerical Method to calculate the State transition matrix ๐ฝ and System/Process noise covariance matrix ๐
[in] | dt | Time step in [s] |
Definition at line 322 of file KeyedKalmanFilter.hpp.
|
inline |
Calculates the state transition matrix ๐ฝ using the exponential matrix.
[in] | tau | Time interval in [s] |
Definition at line 312 of file KeyedKalmanFilter.hpp.
|
inline |
Calculates the state transition matrix ๐ฝ limited to specified order in ๐ ๐โ
[in] | tau | Time interval in [s] |
[in] | order | The order of the Taylor polynom to calculate |
Definition at line 301 of file KeyedKalmanFilter.hpp.
|
inlinenodiscard |
Performs the Normalized Innovation Squared (NIS) test on the measurement innovation ๐น๐ณ
[in] | nameId | Caller 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)
Definition at line 402 of file KeyedKalmanFilter.hpp.
|
inline |
Do a Measurement Update with a Measurement ๐ณ
Definition at line 106 of file KeyedKalmanFilter.hpp.
|
inline |
Do a Measurement Update with a Measurement Innovation ๐น๐ณ
Definition at line 125 of file KeyedKalmanFilter.hpp.
|
inline |
Discards the saved xฬ, ๐, ๐ณ, ๐, ๐ a-priori (pre-update)
Definition at line 378 of file KeyedKalmanFilter.hpp.
|
inlinenodiscard |
Checks if the filter has any of the provided keys.
stateKeys | State keys |
Definition at line 150 of file KeyedKalmanFilter.hpp.
|
inlinenodiscard |
Checks if the filter has the key.
stateKey | State key |
Definition at line 144 of file KeyedKalmanFilter.hpp.
|
inlinenodiscard |
Checks if the filter has the keys.
stateKeys | State keys |
Definition at line 147 of file KeyedKalmanFilter.hpp.
|
inlinenodiscard |
Whether the NIS check should be performed.
Definition at line 385 of file KeyedKalmanFilter.hpp.
|
inlinenodiscard |
Whether a pre-update was saved.
Definition at line 334 of file KeyedKalmanFilter.hpp.
|
inline |
Do a Time Update.
Definition at line 93 of file KeyedKalmanFilter.hpp.
|
inline |
Remove a measurement from the filter.
measKey | Measurement key |
Definition at line 266 of file KeyedKalmanFilter.hpp.
|
inline |
Remove measurements from the filter.
measKeys | Measurement keys |
Definition at line 270 of file KeyedKalmanFilter.hpp.
|
inline |
Remove a state from the filter.
stateKey | State key |
Definition at line 180 of file KeyedKalmanFilter.hpp.
|
inline |
Remove states from the filter.
stateKeys | State keys |
Definition at line 184 of file KeyedKalmanFilter.hpp.
|
inline |
Replace the old with the new key.
[in] | oldKey | Old key to replace |
[in] | newKey | New key to use instead |
Definition at line 207 of file KeyedKalmanFilter.hpp.
|
inline |
Restores the saved xฬ, ๐, ๐ณ, ๐, ๐ a-priori (pre-update)
Definition at line 358 of file KeyedKalmanFilter.hpp.
|
inlinenodiscard |
Accesses the saved pre-update matrices.
Definition at line 547 of file KeyedKalmanFilter.hpp.
|
inline |
Saves xฬ, ๐, ๐ณ, ๐, ๐ a-priori (pre-update)
Definition at line 337 of file KeyedKalmanFilter.hpp.
|
inline |
Sets the measurement keys and initializes matrices z, H, R, S, K with Zero.
measKeys | Measurement keys |
Definition at line 228 of file KeyedKalmanFilter.hpp.
|
inline |
Sets all Vectors and matrices to 0.
Definition at line 74 of file KeyedKalmanFilter.hpp.
|
inline |
Shows GUI elements for the Kalman Filter.
[in] | id | Unique id for ImGui |
[in] | width | Width of the widget |
Definition at line 432 of file KeyedKalmanFilter.hpp.
|
inline |
Shows ImGui Tree nodes for all matrices.
[in] | id | Unique id for ImGui |
[in] | nRows | Amount of rows to show |
Definition at line 457 of file KeyedKalmanFilter.hpp.
|
friend |
Converts the provided json object into a node object.
[in] | j | Json object with the needed values |
[out] | obj | Object to fill from the json |
Definition at line 573 of file KeyedKalmanFilter.hpp.
|
friend |
Converts the provided object into json.
[out] | j | Json object which gets filled with the info |
[in] | obj | Object to convert into json |
Definition at line 563 of file KeyedKalmanFilter.hpp.
|
private |
NIS Test Hypothesis testing failure rate (probability that H_0 is accepted is (1 - alpha))
Definition at line 558 of file KeyedKalmanFilter.hpp.
|
private |
Normalized Innovation Squared (NIS) test.
Definition at line 555 of file KeyedKalmanFilter.hpp.
|
private |
Saved pre-update state and measurement.
Definition at line 552 of file KeyedKalmanFilter.hpp.
KeyedMatrixX<Scalar, StateKeyType, StateKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::F |
๐ System model matrix (n x n)
Definition at line 293 of file KeyedKalmanFilter.hpp.
KeyedMatrixX<Scalar, StateKeyType, StateKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::G |
๐ Noise input matrix (n x o)
Definition at line 294 of file KeyedKalmanFilter.hpp.
KeyedMatrixX<Scalar, MeasKeyType, StateKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::H |
๐ Measurement sensitivity matrix (m x n)
Definition at line 288 of file KeyedKalmanFilter.hpp.
|
private |
๐ฐ Identity matrix (n x n)
Definition at line 550 of file KeyedKalmanFilter.hpp.
KeyedMatrixX<Scalar, StateKeyType, MeasKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::K |
๐ Kalman gain matrix (n x m)
Definition at line 291 of file KeyedKalmanFilter.hpp.
KeyedMatrixX<Scalar, StateKeyType, StateKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::P |
๐ Error covariance matrix (n x n)
Definition at line 284 of file KeyedKalmanFilter.hpp.
KeyedMatrixX<Scalar, StateKeyType, StateKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::Phi |
๐ฝ State transition matrix (n x n)
Definition at line 285 of file KeyedKalmanFilter.hpp.
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.
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.
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.
KeyedMatrixX<Scalar, StateKeyType, StateKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::W |
๐ Noise scale matrix (o x o)
Definition at line 295 of file KeyedKalmanFilter.hpp.
KeyedVectorX<Scalar, StateKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::x |
xฬ State vector (n x 1)
Definition at line 283 of file KeyedKalmanFilter.hpp.
KeyedVectorX<Scalar, MeasKeyType> NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::z |
๐ณ Measurement vector (m x 1)
Definition at line 287 of file KeyedKalmanFilter.hpp.