![]() |
0.4.1
|
Inertial Measurement Integrator. More...
Data Structures | |
struct | GenericState |
Inertial state and measurements. More... | |
struct | Measurement |
Inertial Measurement. More... | |
Public Types | |
enum class | IntegrationAlgorithm : uint8_t { SingleStepRungeKutta1 , SingleStepRungeKutta2 , SingleStepHeun2 , SingleStepRungeKutta3 , SingleStepHeun3 , SingleStepRungeKutta4 , MultiStepRK3 , MultiStepRK4 , COUNT } |
Available Integration Algorithms. More... | |
enum class | IntegrationFrame : uint8_t { ECEF , NED } |
Available Integration Frames. More... | |
using | State |
Inertial state and measurements. | |
Public Member Functions | |
void | addState (const PosVelAtt &state, const char *nameId) |
Pushes the state to the list of states. | |
void | applySensorBiasesIncrements (const Eigen::Vector3d &p_deltaBiasAcceleration, const Eigen::Vector3d &p_deltaBiasAngularRate) |
Sets the sensor biases increment. | |
void | applyStateErrors_e (const Eigen::Vector3d &e_positionError, const Eigen::Vector3d &e_velocityError, const Eigen::Vector3d &e_attitudeError_b) |
Apply the errors to the latest state. | |
void | applyStateErrors_n (const Eigen::Vector3d &lla_positionError, const Eigen::Vector3d &n_velocityError, const Eigen::Vector3d &n_attitudeError_b) |
Apply the errors to the latest state. | |
bool | areAccelerationsAveragedMeasurements () const |
Wether the measurements are accumulated values over the last epoch. (always true when using delta measurements, so GUI has no effect) | |
template<typename T> | |
Eigen::Vector< T, 10 > | calcInertialSolution (const ImuPos &imuPos, const GenericState< T > &state__t0, const GenericState< T > &state__t1, const char *nameId) const |
Calculates the inertial solution going from state__t1 to state__t0 given that measurements are available for both states. | |
std::shared_ptr< PosVelAtt > | calcInertialSolution (const InsTime &obsTime, const Eigen::Vector3d &p_acceleration, const Eigen::Vector3d &p_angularRate, const ImuPos &imuPos, const char *nameId) |
Calculates the inertial navigation solution. | |
std::shared_ptr< PosVelAtt > | calcInertialSolutionDelta (const InsTime &obsTime, double deltaTime, const Eigen::Vector3d &p_deltaVelocity, const Eigen::Vector3d &p_deltaTheta, const ImuPos &imuPos, const char *nameId) |
Calculates the inertial navigation solution. | |
const PosVelAttDerivativeConstants & | getConstants () const |
Returns the selected compensation models. | |
IntegrationFrame | getIntegrationFrame () const |
Returns the selected integration frame. | |
std::optional< std::reference_wrapper< const State > > | getLatestState () const |
Get the latest state if it exists. | |
bool | hasInitialPosition () const |
Checks if an initial position is set. | |
InertialIntegrator ()=default | |
Default Constructor. | |
InertialIntegrator (IntegrationFrame integrationFrame) | |
Constructor. | |
std::optional< Eigen::Vector3d > | p_calcCurrentAcceleration () const |
Calculate the current acceleration, if measurements area available. | |
std::optional< Eigen::Vector3d > | p_calcCurrentAngularRate () const |
Calculate the current angular rate, if measurements area available. | |
const Eigen::Vector3d & | p_getLastAccelerationBias () const |
Return the last acceleration bias in platform frame coordinates in [m/s^2]. | |
const Eigen::Vector3d & | p_getLastAngularRateBias () const |
Return the last angular rate bias in platform frame coordinates in [rad/s]. | |
void | reset () |
Clears all internal data. | |
void | setInitialState (const PosVelAtt &state, const char *nameId) |
Sets the initial state. | |
void | setTotalSensorBiases (const Eigen::Vector3d &p_biasAcceleration, const Eigen::Vector3d &p_biasAngularRate) |
Sets the sensor biases total values. | |
Private Member Functions | |
void | addDeltaMeasurement (const InsTime &epoch, double dt, double deltaTime, const Eigen::Vector3d &p_deltaVelocity, const Eigen::Vector3d &p_deltaTheta, const char *nameId) |
Adds the measurement to the tracking buffer. | |
void | addMeasurement (const InsTime &epoch, double dt, const Eigen::Vector3d &p_acceleration, const Eigen::Vector3d &p_angularRate, const char *nameId) |
Adds the measurement to the tracking buffer. | |
std::shared_ptr< PosVelAtt > | calcInertialSolutionFromMeasurementBuffer (const ImuPos &imuPos, const char *nameId) |
Calculates the inertial navigation solution. | |
std::shared_ptr< PosVelAtt > | lastStateAsPosVelAtt () const |
Returns the last state as PosVelAtt. | |
void | setBufferSizes () |
Resizes the measurement and state buffers depending on the integration algorithm. | |
Private Attributes | |
bool | _accelerationsAreAveragedMeasurements |
If true, then the measurements are accumulated values over the last epoch. (always true when using delta measurements, so GUI has no effect) | |
IntegrationAlgorithm | _integrationAlgorithm |
Integration algorithm used for the update. | |
IntegrationFrame | _integrationFrame |
Frame to integrate the observations in. | |
bool | _lockIntegrationFrame |
Wether to lock the integration frame. | |
Eigen::Vector3d | _p_lastBiasAcceleration |
Initial values for the acceleration bias [m/s^2]. | |
Eigen::Vector3d | _p_lastBiasAngularRate |
Initial values for the angular rate bias [rad/s]. | |
PosVelAttDerivativeConstants | _posVelAttDerivativeConstants |
Settings for the models to use. | |
ScrollingBuffer< State > | _states |
List of states. Length depends on algorithm used. | |
Friends | |
void | from_json (const json &j, InertialIntegrator &data) |
Read info from a json object. | |
bool | InertialIntegratorGui (const char *label, InertialIntegrator &integrator, bool &preferAccelerationOverDeltaMeasurements, float width=250.0F) |
Shows a GUI for advanced configuration of the InertialIntegrator. | |
void | to_json (json &j, const InertialIntegrator &data) |
Write info to a json object. | |
const char * | to_string (InertialIntegrator::IntegrationAlgorithm algorithm) |
Converts the enum to a string. | |
const char * | to_string (InertialIntegrator::IntegrationFrame frame) |
Converts the enum to a string. | |
Inertial Measurement Integrator.
Definition at line 39 of file InertialIntegrator.hpp.
Inertial state and measurements.
Definition at line 95 of file InertialIntegrator.hpp.
|
strong |
Available Integration Algorithms.
Definition at line 43 of file InertialIntegrator.hpp.
|
strong |
Available Integration Frames.
Enumerator | |
---|---|
ECEF | Earth-Centered Earth-Fixed frame. |
NED | Local North-East-Down frame. |
Definition at line 57 of file InertialIntegrator.hpp.
|
default |
Default Constructor.
|
explicit |
Constructor.
integrationFrame | Integration frame to lock to |
Definition at line 26 of file InertialIntegrator.cpp.
|
private |
Adds the measurement to the tracking buffer.
[in] | epoch | Epoch of the measurement |
[in] | dt | Time between observation and last state in [s] |
[in] | deltaTime | Delta time over which the deltaVelocity and deltaTheta were measured in [s] |
[in] | p_deltaVelocity | Integrated acceleration in platform frame coordinates in [m/s] |
[in] | p_deltaTheta | Integrated angular rate in platform frame coordinates in [rad] |
[in] | nameId | NameId of the calling node for logging |
Definition at line 196 of file InertialIntegrator.cpp.
|
private |
Adds the measurement to the tracking buffer.
[in] | epoch | Epoch of the measurement |
[in] | dt | Time between observation and last state in [s] |
[in] | p_acceleration | Acceleration in platform frame coordinates in [m/s^2] |
[in] | p_angularRate | Angular rate in platform frame coordinates in [rad/s] |
[in] | nameId | NameId of the calling node for logging |
Definition at line 162 of file InertialIntegrator.cpp.
void NAV::InertialIntegrator::addState | ( | const PosVelAtt & | state, |
const char * | nameId ) |
Pushes the state to the list of states.
[in] | state | State to set |
[in] | nameId | NameId of the calling node for logging |
Definition at line 48 of file InertialIntegrator.cpp.
void NAV::InertialIntegrator::applySensorBiasesIncrements | ( | const Eigen::Vector3d & | p_deltaBiasAcceleration, |
const Eigen::Vector3d & | p_deltaBiasAngularRate ) |
Sets the sensor biases increment.
[in] | p_deltaBiasAcceleration | Acceleration bias increment in platform frame coordinates in [m/s^2] |
[in] | p_deltaBiasAngularRate | Angular rate bias increment in platform frame coordinates in [rad/s] |
Definition at line 70 of file InertialIntegrator.cpp.
void NAV::InertialIntegrator::applyStateErrors_e | ( | const Eigen::Vector3d & | e_positionError, |
const Eigen::Vector3d & | e_velocityError, | ||
const Eigen::Vector3d & | e_attitudeError_b ) |
Apply the errors to the latest state.
[in] | e_positionError | δr_e The position error in e frame coordinates in [m] |
[in] | e_velocityError | δ𝐯_e The velocity error in e frame coordinates in [m/s] |
[in] | e_attitudeError_b | δ𝛙_eb_e The attitude error in e frame coordinates in [rad] |
Definition at line 105 of file InertialIntegrator.cpp.
void NAV::InertialIntegrator::applyStateErrors_n | ( | const Eigen::Vector3d & | lla_positionError, |
const Eigen::Vector3d & | n_velocityError, | ||
const Eigen::Vector3d & | n_attitudeError_b ) |
Apply the errors to the latest state.
[in] | lla_positionError | δ𝐩_n = [δ𝜙 δλ δ𝘩] The position error (latitude, longitude, altitude) in [rad, rad, m] |
[in] | n_velocityError | δ𝐯_n The velocity error in n frame coordinates in [m/s] |
[in] | n_attitudeError_b | δ𝛙_nb_n The attitude error in n frame coordinates in [rad] |
Definition at line 76 of file InertialIntegrator.cpp.
|
nodiscard |
Wether the measurements are accumulated values over the last epoch. (always true when using delta measurements, so GUI has no effect)
Definition at line 143 of file InertialIntegrator.cpp.
|
inlinenodiscard |
Calculates the inertial solution going from state__t1 to state__t0 given that measurements are available for both states.
imuPos | IMU mounting position connecting the platform to the body frame |
state__t0 | State at the epoch to calculate (measurements only) |
state__t1 | State at the previous epoch (state + measurements) |
nameId | NameId of the calling node for logging |
Definition at line 200 of file InertialIntegrator.hpp.
std::shared_ptr< PosVelAtt > NAV::InertialIntegrator::calcInertialSolution | ( | const InsTime & | obsTime, |
const Eigen::Vector3d & | p_acceleration, | ||
const Eigen::Vector3d & | p_angularRate, | ||
const ImuPos & | imuPos, | ||
const char * | nameId ) |
Calculates the inertial navigation solution.
[in] | obsTime | Time of the observation |
[in] | p_acceleration | Acceleration in platform frame coordinates in [m/s^2] |
[in] | p_angularRate | Angular rate in platform frame coordinates in [rad/s] |
[in] | imuPos | IMU platform frame position with respect to body frame |
[in] | nameId | NameId of the calling node for logging |
Definition at line 237 of file InertialIntegrator.cpp.
std::shared_ptr< PosVelAtt > NAV::InertialIntegrator::calcInertialSolutionDelta | ( | const InsTime & | obsTime, |
double | deltaTime, | ||
const Eigen::Vector3d & | p_deltaVelocity, | ||
const Eigen::Vector3d & | p_deltaTheta, | ||
const ImuPos & | imuPos, | ||
const char * | nameId ) |
Calculates the inertial navigation solution.
[in] | obsTime | Time of the observation |
[in] | deltaTime | Delta time over which the deltaVelocity and deltaTheta were measured in [s] |
[in] | p_deltaVelocity | Integrated acceleration in platform frame coordinates in [m/s] |
[in] | p_deltaTheta | Integrated angular rate in platform frame coordinates in [rad] |
[in] | imuPos | IMU platform frame position with respect to body frame |
[in] | nameId | NameId of the calling node for logging |
Definition at line 255 of file InertialIntegrator.cpp.
|
private |
Calculates the inertial navigation solution.
[in] | imuPos | IMU platform frame position with respect to body frame |
[in] | nameId | NameId of the calling node for logging |
Definition at line 273 of file InertialIntegrator.cpp.
|
nodiscard |
Returns the selected compensation models.
Definition at line 138 of file InertialIntegrator.cpp.
|
nodiscard |
Returns the selected integration frame.
Definition at line 133 of file InertialIntegrator.cpp.
|
nodiscard |
Get the latest state if it exists.
Definition at line 117 of file InertialIntegrator.cpp.
|
nodiscard |
Checks if an initial position is set.
Definition at line 37 of file InertialIntegrator.cpp.
|
nodiscardprivate |
Returns the last state as PosVelAtt.
Definition at line 218 of file InertialIntegrator.cpp.
|
nodiscard |
Calculate the current acceleration, if measurements area available.
Definition at line 148 of file InertialIntegrator.cpp.
|
nodiscard |
Calculate the current angular rate, if measurements area available.
Definition at line 155 of file InertialIntegrator.cpp.
|
nodiscard |
Return the last acceleration bias in platform frame coordinates in [m/s^2].
Definition at line 123 of file InertialIntegrator.cpp.
|
nodiscard |
Return the last angular rate bias in platform frame coordinates in [rad/s].
Definition at line 128 of file InertialIntegrator.cpp.
void NAV::InertialIntegrator::reset | ( | ) |
Clears all internal data.
Definition at line 29 of file InertialIntegrator.cpp.
|
private |
Resizes the measurement and state buffers depending on the integration algorithm.
Definition at line 312 of file InertialIntegrator.cpp.
void NAV::InertialIntegrator::setInitialState | ( | const PosVelAtt & | state, |
const char * | nameId ) |
Sets the initial state.
[in] | state | State to set |
[in] | nameId | NameId of the calling node for logging |
Definition at line 42 of file InertialIntegrator.cpp.
void NAV::InertialIntegrator::setTotalSensorBiases | ( | const Eigen::Vector3d & | p_biasAcceleration, |
const Eigen::Vector3d & | p_biasAngularRate ) |
Sets the sensor biases total values.
[in] | p_biasAcceleration | Acceleration bias in platform frame coordinates in [m/s^2] |
[in] | p_biasAngularRate | Angular rate bias in platform frame coordinates in [rad/s] |
Definition at line 64 of file InertialIntegrator.cpp.
|
friend |
Read info from a json object.
[in] | j | Json variable to read info from |
[out] | data | Output object |
Definition at line 444 of file InertialIntegrator.cpp.
|
friend |
Shows a GUI for advanced configuration of the InertialIntegrator.
[in] | label | Label to show. This has to be a unique id for ImGui. |
[in] | integrator | Reference to the integrator to configure |
[in] | preferAccelerationOverDeltaMeasurements | Wether to prefer accelerations over delta measurements |
[in] | width | Width of the widget |
Definition at line 333 of file InertialIntegrator.cpp.
|
friend |
Write info to a json object.
[out] | j | Json output |
[in] | data | Object to read info from |
Definition at line 433 of file InertialIntegrator.cpp.
|
friend |
Converts the enum to a string.
[in] | algorithm | Enum value to convert into text |
Definition at line 457 of file InertialIntegrator.cpp.
|
friend |
Converts the enum to a string.
[in] | frame | Enum value to convert into text |
Definition at line 483 of file InertialIntegrator.cpp.
|
private |
If true, then the measurements are accumulated values over the last epoch. (always true when using delta measurements, so GUI has no effect)
Definition at line 490 of file InertialIntegrator.hpp.
|
private |
Integration algorithm used for the update.
Definition at line 487 of file InertialIntegrator.hpp.
|
private |
Frame to integrate the observations in.
Definition at line 481 of file InertialIntegrator.hpp.
|
private |
Wether to lock the integration frame.
Definition at line 484 of file InertialIntegrator.hpp.
|
private |
Initial values for the acceleration bias [m/s^2].
Definition at line 475 of file InertialIntegrator.hpp.
|
private |
Initial values for the angular rate bias [rad/s].
Definition at line 476 of file InertialIntegrator.hpp.
|
private |
Settings for the models to use.
Definition at line 495 of file InertialIntegrator.hpp.
|
private |
List of states. Length depends on algorithm used.
Definition at line 473 of file InertialIntegrator.hpp.