class mola::NavStateFuse
Overview
Fuse of odometry, IMU, and SE(3) pose/twist estimations.
Usage:
Call initialize() or set the required parameters directly in params_.
Integrate measurements with
fuse_*()
methods. Each CObservation class includes atimestamp
field which is used to estimate the trajectory.
Repeat (2) as needed.
Read the estimation up to any nearby moment in time with estimated_navstate()
Old observations are automatically removed.
See also:
IMUIntegrator
#include <NavStateFuse.h> class NavStateFuse { public: // structs struct State; // fields NavStateFuseParams params_; // methods void initialize(const mrpt::containers::yaml& cfg); void reset(); void fuse_odometry(const mrpt::obs::CObservationOdometry& odom); void fuse_imu(const mrpt::obs::CObservationIMU& imu); void fuse_pose( const mrpt::Clock::time_point& timestamp, const mrpt::poses::CPose3DPDFGaussian& pose ); void fuse_twist( const mrpt::Clock::time_point& timestamp, const mrpt::math::TTwist3D& twist ); std::optional<NavState> estimated_navstate(const mrpt::Clock::time_point& timestamp) const; std::optional<mrpt::math::TTwist3D> get_last_twist() const; void force_last_twist(const mrpt::math::TTwist3D& twist); };
Methods
void initialize(const mrpt::containers::yaml& cfg)
Initializes the object and reads all parameters from a YAML node.
Parameters:
cfg |
a YAML node with a dictionary of parameters to load from. |
void reset()
Resets the estimator state to an initial state.
See also:
currentIntegrationState
void fuse_odometry(const mrpt::obs::CObservationOdometry& odom)
Integrates new odometry observations into the estimator
void fuse_imu(const mrpt::obs::CObservationIMU& imu)
Integrates new IMU observations into the estimator
void fuse_pose( const mrpt::Clock::time_point& timestamp, const mrpt::poses::CPose3DPDFGaussian& pose )
Integrates new SE(3) pose estimation into the estimator
void fuse_twist( const mrpt::Clock::time_point& timestamp, const mrpt::math::TTwist3D& twist )
Integrates new twist estimation (in the odom frame)
std::optional<NavState> estimated_navstate(const mrpt::Clock::time_point& timestamp) const
Computes the estimated vehicle state at a given timestep using the observations in the time window. A std::nullopt is returned if there is no valid observations yet, or if requested a timestamp out of the model validity time window (e.g. too far in the future to be trustful).