17#include <boost/math/distributions/chi_squared.hpp>
37template<
typename Scalar,
typename StateKeyType,
typename MeasKeyType>
47 KeyedKalmanFilter(std::span<const StateKeyType> stateKeys, std::span<const MeasKeyType> measKeys)
49 std::unordered_set<StateKeyType> stateSet = { stateKeys.begin(), stateKeys.end() };
51 std::unordered_set<MeasKeyType> measSet = { measKeys.begin(), measKeys.end() };
54 auto n =
static_cast<int>(stateKeys.size());
55 auto m =
static_cast<int>(measKeys.size());
70 I = Eigen::MatrixX<Scalar>::Identity(n, n);
144 [[nodiscard]]
bool hasState(
const StateKeyType& stateKey)
const {
return x.hasRow(stateKey); }
147 [[nodiscard]]
bool hasStates(std::span<const StateKeyType> stateKeys)
const {
return x.hasStates(stateKeys); }
150 [[nodiscard]]
bool hasAnyStates(std::span<const StateKeyType> stateKeys)
const {
return x.hasAnyStates(stateKeys); }
154 void addState(
const StateKeyType& stateKey) {
addStates(std::initializer_list<StateKeyType>{ stateKey }); }
160 INS_ASSERT_USER_ERROR(!
x.hasAnyRows(stateKeys),
"You cannot add a state key which is already in the Kalman filter.");
161 std::unordered_set<StateKeyType> stateSet = { stateKeys.begin(), stateKeys.end() };
164 auto n =
x(
all).rows() +
static_cast<int>(stateKeys.size());
166 x.addRows(stateKeys);
167 P.addRowsCols(stateKeys, stateKeys);
168 F.addRowsCols(stateKeys, stateKeys);
169 Phi.addRowsCols(stateKeys, stateKeys);
170 G.addRowsCols(stateKeys, stateKeys);
171 W.addRowsCols(stateKeys, stateKeys);
172 Q.addRowsCols(stateKeys, stateKeys);
173 H.addCols(stateKeys);
174 K.addRows(stateKeys);
175 I = Eigen::MatrixX<Scalar>::Identity(n, n);
186 INS_ASSERT_USER_ERROR(
x.hasRows(stateKeys),
"Not all state keys you are trying to remove are in the Kalman filter.");
187 std::unordered_set<StateKeyType> stateSet = { stateKeys.begin(), stateKeys.end() };
190 auto n =
x.rows() -
static_cast<int>(stateKeys.size());
192 x.removeRows(stateKeys);
193 P.removeRowsCols(stateKeys, stateKeys);
194 F.removeRowsCols(stateKeys, stateKeys);
195 Phi.removeRowsCols(stateKeys, stateKeys);
196 G.removeRowsCols(stateKeys, stateKeys);
197 W.removeRowsCols(stateKeys, stateKeys);
198 Q.removeRowsCols(stateKeys, stateKeys);
199 H.removeCols(stateKeys);
200 K.removeRows(stateKeys);
201 I = Eigen::MatrixX<Scalar>::Identity(n, n);
207 void replaceState(
const StateKeyType& oldKey,
const StateKeyType& newKey)
209 x.replaceRowKey(oldKey, newKey);
210 P.replaceRowKey(oldKey, newKey);
211 P.replaceColKey(oldKey, newKey);
212 F.replaceRowKey(oldKey, newKey);
213 F.replaceColKey(oldKey, newKey);
214 Phi.replaceRowKey(oldKey, newKey);
215 Phi.replaceColKey(oldKey, newKey);
216 G.replaceRowKey(oldKey, newKey);
217 G.replaceColKey(oldKey, newKey);
218 W.replaceRowKey(oldKey, newKey);
219 W.replaceColKey(oldKey, newKey);
220 Q.replaceRowKey(oldKey, newKey);
221 Q.replaceColKey(oldKey, newKey);
222 H.replaceColKey(oldKey, newKey);
223 K.replaceRowKey(oldKey, newKey);
230 std::unordered_set<MeasKeyType> measSet = { measKeys.begin(), measKeys.end() };
233 auto n =
static_cast<int>(
x.rows());
234 auto m =
static_cast<int>(measKeys.size());
236 const auto& stateKeys =
x.rowKeys();
253 INS_ASSERT_USER_ERROR(!
z.hasAnyRows(measKeys),
"A measurement keys you are trying to add was already in the Kalman filter.");
254 std::unordered_set<MeasKeyType> measurementSet = { measKeys.begin(), measKeys.end() };
255 INS_ASSERT_USER_ERROR(measurementSet.size() == measKeys.size(),
"Each measurement key must be unique");
259 R.addRowsCols(measKeys, measKeys);
260 S.addRowsCols(measKeys, measKeys);
272 INS_ASSERT_USER_ERROR(
z.hasRows(measKeys),
"Not all measurement keys you are trying to remove are in the Kalman filter.");
273 std::unordered_set<MeasKeyType> measurementSet = { measKeys.begin(), measKeys.end() };
274 INS_ASSERT_USER_ERROR(measurementSet.size() == measKeys.size(),
"Each measurement key must be unique");
276 z.removeRows(measKeys);
277 H.removeRows(measKeys);
278 R.removeRowsCols(measKeys, measKeys);
279 S.removeRowsCols(measKeys, measKeys);
280 K.removeCols(measKeys);
303 INS_ASSERT_USER_ERROR(
F.rowKeys() ==
Phi.rowKeys(),
"The system model matrix F and the state transition matrix ๐ฝ need to have the same keys.");
314 INS_ASSERT_USER_ERROR(
F.rowKeys() ==
Phi.rowKeys(),
"The system model matrix F and the state transition matrix ๐ฝ need to have the same keys.");
324 INS_ASSERT_USER_ERROR(
G.colKeys() ==
W.rowKeys(),
"The columns of the noise input matrix G and rows of the noise scale matrix W must match. (G * W * G^T)");
325 INS_ASSERT_USER_ERROR(
G.rowKeys() ==
Q.rowKeys(),
"The rows of the noise input matrix G and the System/Process noise covariance matrix Q must match.");
326 INS_ASSERT_USER_ERROR(
G.colKeys() ==
Q.colKeys(),
"The cols of the noise input matrix G and the System/Process noise covariance matrix Q must match.");
405 if (
z.rows() == 0) {
return ret; }
408 ret.NIS = std::abs(
z(
all).transpose() *
S(
all,
all).inverse() *
z(
all));
410 boost::math::chi_squared dist(
static_cast<double>(
z.rows()));
412 ret.r2 = boost::math::quantile(dist, 1.0 -
_alphaNIS);
414 ret.triggered = ret.NIS >= ret.r2;
418 LOG_TRACE(
"{}: NIS test triggered because: NIS = {:.3f} > {:.3f} = r2", nameId, ret.NIS, ret.r2);
422 LOG_DATA(
"{}: NIS test passed: NIS = {:.3f} <= {:.3f} = r2", nameId, ret.NIS, ret.r2);
434 bool changed =
false;
436 changed |= ImGui::Checkbox(fmt::format(
"Enable outlier NIS check##{}",
id).c_str(), &
_checkNIS);
443 ImGui::SetNextItemWidth(width - ImGui::GetStyle().IndentSpacing);
444 if (
ImGui::DragDouble(fmt::format(
"NIS alpha (failure rate)##{}",
id).c_str(), &alpha, 1.0F, 0.0, 100.0,
"%.2f %%"))
459 ImGui::PushFont(Application::MonoFont());
460 float matrixTableHeight = ImGui::GetTextLineHeightWithSpacing() *
static_cast<float>(nRows + 1);
461 float vectorTableHeight = ImGui::GetTextLineHeightWithSpacing() *
static_cast<float>(nRows);
464 if (ImGui::TreeNode(fmt::format(
"x - State vector##{}",
id).c_str()))
469 if (ImGui::TreeNode(fmt::format(
"P - Error covariance matrix##{}",
id).c_str()))
474 if (ImGui::TreeNode(fmt::format(
"Phi - State transition matrix##{}",
id).c_str()))
479 if (ImGui::TreeNode(fmt::format(
"Q System/Process noise covariance matrix##{}",
id).c_str()))
484 if (ImGui::TreeNode(fmt::format(
"z - Measurement vector##{}",
id).c_str()))
489 if (ImGui::TreeNode(fmt::format(
"H - Measurement sensitivity matrix##{}",
id).c_str()))
494 if (ImGui::TreeNode(fmt::format(
"R - Measurement noise covariance matrix##{}",
id).c_str()))
499 if (ImGui::TreeNode(fmt::format(
"S - Measurement prediction covariance matrix##{}",
id).c_str()))
504 if (ImGui::TreeNode(fmt::format(
"K - Kalman gain matrix##{}",
id).c_str()))
509 if (ImGui::TreeNode(fmt::format(
"F - System model matrix##{}",
id).c_str()))
514 if (ImGui::TreeNode(fmt::format(
"G - Noise input matrix##{}",
id).c_str()))
519 if (ImGui::TreeNode(fmt::format(
"W - Noise scale matrix##{}",
id).c_str()))
575 if (j.contains(
"checkNIS")) { j.at(
"checkNIS").get_to(obj.
_checkNIS); }
576 if (j.contains(
"alphaNIS")) { j.at(
"alphaNIS").get_to(obj.
_alphaNIS); }
583template<
typename StateKeyType,
typename MeasKeyType>
591template<
typename Scalar,
typename RowKeyType,
typename ColKeyType,
int Rows,
int Cols>
592KeyedMatrix<Scalar, RowKeyType, ColKeyType, Rows, Cols> transitionMatrix_Phi_Taylor(
const KeyedMatrix<Scalar, RowKeyType, ColKeyType, Rows, Cols>& F, Scalar tau_s,
size_t order)
595 Eigen::Matrix<Scalar, Rows, Cols> Phi;
597 if constexpr (Rows == Eigen::Dynamic)
599 Phi = Eigen::Matrix<Scalar, Rows, Cols>::Identity(F.
rows(), F.
cols());
603 Phi = Eigen::Matrix<Scalar, Rows, Cols>::Identity();
607 for (
size_t i = 1; i <= order; i++)
609 Eigen::Matrix<Scalar, Rows, Cols> Fpower = F(
all,
all);
612 for (
size_t j = 1; j < i; j++)
631template<
typename Scalar,
typename RowKeyType,
typename ColKeyType,
int Rows,
int Cols>
#define INS_ASSERT_USER_ERROR(_EXP, _MSG)
Assert function with message.
nlohmann::json json
json namespace
Text Help Marker (?) with Tooltip.
The class is responsible for all time-related tasks.
Defines how to save certain datatypes to json.
Utility class for logging to console and file.
#define LOG_DATA
All output which occurs repeatedly every time observations are received.
#define LOG_TRACE
Detailled info to trace the execution of the program. Should not be called on functions which receive...
Van Loan Method loan1978.
Keyed Kalman Filter class.
KeyedMatrixX< double, MeasKeyType, StateKeyType > H
bool showKalmanFilterGUI(const char *id, float width=0.0F)
Shows GUI elements for the Kalman Filter.
void calcPhiAndQWithVanLoanMethod(Scalar dt)
Numerical Method to calculate the State transition matrix ๐ฝ and System/Process noise covariance matri...
void replaceState(const StateKeyType &oldKey, const StateKeyType &newKey)
Replace the old with the new key.
void addStates(std::span< const StateKeyType > stateKeys)
Add new states to the filter.
KeyedVectorX< double, MeasKeyType > z
void discardPreUpdate()
Discards the saved xฬ, ๐, ๐ณ, ๐, ๐ a-priori (pre-update)
bool isPreUpdateSaved() const
Whether a pre-update was saved.
bool hasState(const StateKeyType &stateKey) const
Checks if the filter has the key.
void removeState(const StateKeyType &stateKey)
Remove a state from the filter.
friend void from_json(const json &j, KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType > &obj)
Converts the provided json object into a node object.
auto checkForOutliersNIS(const std::string &nameId)
Performs the Normalized Innovation Squared (NIS) test on the measurement innovation ๐น๐ณ
void removeMeasurements(std::span< const MeasKeyType > measKeys)
Remove measurements from the filter.
KeyedMatrixX< double, MeasKeyType, MeasKeyType > S
KeyedMatrixX< double, StateKeyType, MeasKeyType > K
friend void to_json(json &j, const KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType > &obj)
Converts the provided object into json.
KeyedKalmanFilter(std::span< const StateKeyType > stateKeys, std::span< const MeasKeyType > measKeys)
Constructor.
KeyedVectorX< double, StateKeyType > x
KeyedMatrixX< double, StateKeyType, StateKeyType > W
void addMeasurement(const MeasKeyType &measKey)
Add a measurement to the filter.
void removeStates(std::span< const StateKeyType > stateKeys)
Remove states from the filter.
KeyedMatrixX< double, StateKeyType, StateKeyType > F
void removeMeasurement(const MeasKeyType &measKey)
Remove a measurement from the filter.
void showKalmanFilterMatrixViews(const char *id, int nRows=-2)
Shows ImGui Tree nodes for all matrices.
KeyedMatrixX< double, StateKeyType, StateKeyType > Q
void addState(const StateKeyType &stateKey)
Add a new state to the filter.
void restorePreUpdate()
Restores the saved xฬ, ๐, ๐ณ, ๐, ๐ a-priori (pre-update)
void setZero()
Sets all Vectors and matrices to 0.
bool hasStates(std::span< const StateKeyType > stateKeys) const
Checks if the filter has the keys.
bool hasAnyStates(std::span< const StateKeyType > stateKeys) const
Checks if the filter has any of the provided keys.
void correctWithMeasurementInnovation()
Do a Measurement Update with a Measurement Innovation ๐น๐ณ
void predict()
Do a Time Update.
bool isNISenabled() const
Whether the NIS check should be performed.
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 ๐
๐โ
void savePreUpdate()
Saves xฬ, ๐, ๐ณ, ๐, ๐ a-priori (pre-update)
void correct()
Do a Measurement Update with a Measurement ๐ณ
KeyedMatrixX< double, StateKeyType, StateKeyType > Phi
KeyedKalmanFilter()=default
Default Constructor.
KeyedMatrixX< double, MeasKeyType, MeasKeyType > R
void addMeasurements(std::span< const MeasKeyType > measKeys)
Add measurements to the filter.
void setMeasurements(std::span< const MeasKeyType > measKeys)
Sets the measurement keys and initializes matrices z, H, R, S, K with Zero.
KeyedMatrixX< double, StateKeyType, StateKeyType > P
const SavedPreUpdate & savedPreUpdate() const
Accesses the saved pre-update matrices.
SavedPreUpdate _savedPreUpdate
KeyedMatrixX< double, StateKeyType, StateKeyType > G
Static sized KeyedMatrix.
const std::vector< ColKeyType > & colKeys() const
Returns the col keys.
decltype(auto) cols() const
Return the cols of the underlying Eigen matrix.
decltype(auto) rows() const
Return the rows of the underlying Eigen matrix.
const std::vector< RowKeyType > & rowKeys() const
Returns the row keys.
bool DragDouble(const char *label, double *v, float v_speed, double v_min, double v_max, const char *format, ImGuiSliderFlags flags)
Shows a Drag GUI element for 'double'.
uint64_t factorial(uint64_t n)
Calculates the factorial of an unsigned integer.
std::pair< typename DerivedF::PlainObject, typename DerivedF::PlainObject > calcPhiAndQWithVanLoanMethod(const Eigen::MatrixBase< DerivedF > &F, const Eigen::MatrixBase< DerivedG > &G, const Eigen::MatrixBase< DerivedW > &W, double dt)
Numerical Method to calculate the State transition matrix ๐ฝ and System/Process noise covariance matri...
Derived::PlainObject transitionMatrix_Phi_exp(const Eigen::MatrixBase< Derived > &F, typename Derived::Scalar tau_s)
Calculates the state transition matrix ๐ฝ using the exponential matrix.
KeyedMatrix< Scalar, RowKeyType, ColKeyType, Eigen::Dynamic, Eigen::Dynamic > KeyedMatrixX
Dynamic size KeyedMatrix.
Derived::PlainObject transitionMatrix_Phi_Taylor(const Eigen::MatrixBase< Derived > &F, double tau_s, size_t order)
Calculates the state transition matrix ๐ฝ limited to specified order in ๐
๐โ
KeyedKalmanFilter< double, StateKeyType, MeasKeyType > KeyedKalmanFilterD
Keyed Kalman Filter class with double as type.
KeyedVector< Scalar, RowKeyType, Eigen::Dynamic > KeyedVectorX
Dynamic size KeyedVector.
static const internal::all_t all
Used to request all rows or columns in KeyedMatrices.
Normalized Innovation Squared (NIS) test results.
double NIS
Normalized Innovation Squared (NIS)
bool triggered
Whether the test triggered.
double r2
Upper boundary of one-sided acceptance interval.
Saved pre-update state and measurement.
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > F
๐
System model matrix (n x n)
KeyedMatrixX< Scalar, MeasKeyType, StateKeyType > H
๐ Measurement sensitivity matrix (m x n)
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > Q
๐ System/Process noise covariance matrix (n x n)
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > G
๐ Noise input matrix (n x o)
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > P
๐ Error covariance matrix (n x n)
KeyedMatrixX< Scalar, StateKeyType, MeasKeyType > K
KeyedMatrixX< Scalar, StateKeyType, StateKeyType > Phi
๐ฝ State transition matrix (n x n)
bool saved
Flag whether the state was saved.
KeyedVectorX< Scalar, MeasKeyType > z
๐ณ Measurement vector (m x 1)
KeyedMatrixX< Scalar, MeasKeyType, MeasKeyType > R
๐ = ๐ธ{๐ฐโ๐ฐโแต} Measurement noise 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)
KeyedMatrixX< Scalar, MeasKeyType, MeasKeyType > S
๐ฆ Measurement prediction covariance matrix (m x m)
Matrix which can be accessed by keys.