22#include <fmt/ranges.h>
56template<
typename StateKeyType>
59template<
typename StateKeyType>
62template<
typename StateKeyType>
66template<
typename StateKeyType>
70template<
typename StateKeyType>
78template<
typename StateKeyType>
95 template<
typename Scalar,
int Size>
101 F.template block<3>(
Pos,
Vel) = Eigen::Matrix3d::Identity();
115 template<
typename Scalar,
int Size>
122 const double& latitude,
123 const double& longitude,
160 updatePhiAndQ(Phi, Q, G, F, W, dt, latitude, longitude, algorithm);
170 bool ShowGui(
float itemWidth,
float unitWidth,
const char*
id)
172 bool changed =
false;
182 "%.2e", ImGuiInputTextFlags_CharsScientific))
190 "- 1 {} for a pedestrian or ship,\n"
191 "- {} {} for a car, and\n"
192 "- {} {} for a military aircraft.\n"
193 "The vertical acceleration PSD is usually smaller.",
221 Eigen::Matrix3d a_S_n = Eigen::DiagonalMatrix<double, 3>(
_covarianceAccel[0],
227 double dt2 = std::pow(dt, 2);
228 double dt3 = dt2 * dt;
233 Q.template block<3>(
Pos,
Pos) = dt3 / 3.0 * a_S_e;
234 Q.template block<3>(
Pos,
Vel) = dt2 / 2.0 * a_S_e;
235 Q.template block<3>(
Vel,
Pos) = Q.template block<3>(
Pos,
Vel).transpose();
236 Q.template block<3>(
Vel,
Vel) = dt * a_S_e;
266 if (j.contains(
"covarianceAccel")) { j.at(
"covarianceAccel").get_to(data.
_gui_covarianceAccel); }
278#ifndef DOXYGEN_IGNORE
282struct fmt::formatter<
NAV::Keys::MotionModelKey> : fmt::formatter<const char*>
288 template<
typename FormatContext>
291 using namespace NAV::Keys;
296 return fmt::formatter<const char*>::format(
"PosX", ctx);
298 return fmt::formatter<const char*>::format(
"PosY", ctx);
300 return fmt::formatter<const char*>::format(
"PosZ", ctx);
302 return fmt::formatter<const char*>::format(
"VelX", ctx);
304 return fmt::formatter<const char*>::format(
"VelY", ctx);
306 return fmt::formatter<const char*>::format(
"VelZ", ctx);
308 return fmt::formatter<const char*>::format(
"AttQ1", ctx);
310 return fmt::formatter<const char*>::format(
"AttQ2", ctx);
312 return fmt::formatter<const char*>::format(
"AttQ3", ctx);
314 return fmt::formatter<const char*>::format(
"AttQ0", ctx);
316 return fmt::formatter<const char*>::format(
"MotionModelKey_COUNT", ctx);
319 return fmt::formatter<const char*>::format(
"ERROR", ctx);
Transformation collection.
nlohmann::json json
json namespace
Units used by the system model parameters.
Text Help Marker (?) with Tooltip.
#define LOG_DEBUG
Debug information. Should not be called on functions which receive observations (spamming)
Generalized Kalman Filter class.
std::ostream & operator<<(std::ostream &os, const NAV::Keys::MotionModelKey &obj)
Stream insertion operator overload.
Van Loan Method loan1978.
Static sized KeyedMatrix.
friend void to_json(json &j, const MotionModel &data)
Converts the provided data into a json object.
const std::array< StateKeyType, 6 > & PosVel
All position and velocity keys.
void updatePhiAndQ(KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &Phi, KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &Q, KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &G, const KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &F, const KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &W, double dt, const double &latitude, const double &longitude, SystemModelCalcAlgorithm algorithm)
Updates the provided Phi, Q and G matrix.
std::array< double, 2 > _covarianceAccel
Covariance of the acceleration 𝜎_a due to user motion in horizontal and vertical component [m²/s³].
std::array< double, 2 > _gui_covarianceAccel
GUI selection for the Standard deviation of the acceleration 𝜎_a due to user motion in horizontal and...
void initialize()
Initializes the motion model.
const std::array< StateKeyType, 3 > & Vel
All velocity keys.
void initialize(KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &F, KeyedMatrix< Scalar, StateKeyType, StateKeyType, Size, Size > &W)
Initializes the motion model.
bool ShowGui(float itemWidth, float unitWidth, const char *id)
Shows a GUI.
Units::CovarianceAccelUnits _gui_covarianceAccelUnit
Gui selection for the Unit of the input covarianceAccel parameter for the StDev due to acceleration d...
const std::array< StateKeyType, 3 > & Pos
All position keys.
KeyedMatrix6d< StateKeyType > calcProcessNoiseMatrixTaylor(double dt, double latitude, double longitude) const
Calculates the process noise matrix Q.
friend void from_json(const json &j, MotionModel &data)
Converts the provided json object into the data object.
std::pair< KeyedMatrix6d< StateKeyType >, KeyedMatrix6d< StateKeyType > > calcPhiAndQ(double dt, const double &latitude, const double &longitude, SystemModelCalcAlgorithm algorithm)
Calculates the state transition matrix (𝚽) and the process noise covariance matrix (𝐐)
constexpr std::array< StateKeyType, 4 > Att
All attitude keys.
constexpr std::array< StateKeyType, 3 > Pos
All position keys.
MotionModelKey
Keys used in the model.
@ PosY
Position ECEF_Y [m].
@ VelZ
Velocity ECEF_Z [m/s].
@ VelY
Velocity ECEF_Y [m/s].
@ VelX
Velocity ECEF_X [m/s].
@ AttQ0
w: Real (scalar) part of the Quaternion
@ PosX
Position ECEF_X [m].
@ AttQ3
z: Coefficient of k
@ AttQ1
x: Coefficient of i
@ AttQ2
y: Coefficient of j
@ PosZ
Position ECEF_Z [m].
@ MotionModelKey_COUNT
Count.
constexpr std::array< StateKeyType, 10 > PosVelAtt
Vector with all position velocity and attitude keys.
constexpr std::array< StateKeyType, 6 > PosVel
Vector with all position and velocity keys.
constexpr std::array< StateKeyType, 3 > Vel
All velocity keys.
CovarianceAccelUnits
Possible Units for the Standard deviation of the acceleration due to user motion.
Eigen::Quaternion< Scalar > e_Quat_n(const Scalar &latitude, const Scalar &longitude)
Quaternion for rotations from navigation to Earth-fixed frame.
Eigen::Quaternion< Scalar > n_Quat_e(const Scalar &latitude, const Scalar &longitude)
Quaternion for rotations from Earth-fixed to navigation frame.
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...
const char * to_string(gui::widgets::PositionWithFrame::ReferenceFrame refFrame)
Converts the enum to a string.
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 𝐅𝜏ₛ
double convertUnit(const double &value, Units::PositionUncertaintyUnits unit)
Converts the value depending on the unit provided.
std::string MakeComboItems()
Units separated by '\0' and terminated by double '\0'.
static const internal::all_t all
Used to request all rows or columns in KeyedMatrices.
SystemModelCalcAlgorithm
Algorithms to calculate the system model with.
KeyedMatrix< double, RowKeyType, ColKeyType, 6, 6 > KeyedMatrix6d
Static 6x6 squared size KeyedMatrix with double types.
Matrix which can be accessed by keys.