Class StateEstimationSimple

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

  • public mola::NavStateFilter

Class Documentation

class StateEstimationSimple : public mola::NavStateFilter

Fuse of odometry, IMU, and SE(3) pose/twist estimations.

Usage:

  • (1) Call initialize() or set the required parameters directly in params_.

  • (2) Integrate measurements with fuse_*() methods. Each CObservation class includes a timestamp field which is used to estimate the trajectory.

  • (3) Repeat (2) as needed.

  • (4) Read the estimation up to any nearby moment in time with estimated_navstate()

Old observations are automatically removed.

See also

mola::IMUIntegrator

Note

This implementation of mola::NavStateFilter ignores the passed “frame_id”.

Note

It also ignore GNSS sensor.

Main API

Parameters params
void initialize(const mrpt::containers::yaml &cfg) override

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 spinOnce() override
void reset() override

Resets the estimator state to an initial state.

See also

currentIntegrationState

void fuse_pose(const mrpt::Clock::time_point &timestamp, const mrpt::poses::CPose3DPDFGaussian &pose, const std::string &frame_id) override

Integrates new SE(3) pose estimation of the vehicle wrt frame_id

void fuse_odometry(const mrpt::obs::CObservationOdometry &odom, const std::string &odomName = "odom_wheels") override

Integrates new wheels-based odometry observations into the estimator. This is a convenience method that internally ends up calling fuse_pose(), but computing the uncertainty of odometry increments according to a given motion model.

void fuse_imu(const mrpt::obs::CObservationIMU &imu) override

Integrates new IMU observations into the estimator

void fuse_gnss(const mrpt::obs::CObservationGPS &gps) override

Integrates new GNSS observations into the estimator

void fuse_twist(const mrpt::Clock::time_point &timestamp, const mrpt::math::TTwist3D &twist, const mrpt::math::CMatrixDouble66 &twistCov) override

Integrates new twist estimation (in the odom frame)

std::optional<NavState> estimated_navstate(const mrpt::Clock::time_point &timestamp, const std::string &frame_id) override

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).

std::optional<mrpt::math::TTwist3D> get_last_twist() const

Public Functions

StateEstimationSimple()

Protected Functions

void onNewObservation(const CObservation::Ptr &o) override