class mola::NavStateFuse

Overview

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.

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

See also:

IMUIntegrator

#include <NavStateFuse.h>

class NavStateFuse: public mola::NavStateFilter
{
public:
    // structs

    struct State;

    // fields

    NavStateFuseParams params_;

    // methods

    virtual void initialize(const mrpt::containers::yaml& cfg);
    virtual void reset();

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

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

    virtual void fuse_imu(const mrpt::obs::CObservationIMU& imu);
    virtual void fuse_gnss(const mrpt::obs::CObservationGPS& gps);

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

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

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

Inherited Members

public:
    // structs

    struct GeoReferenceParams;

    // methods

    virtual void reset() = 0;
    virtual void initialize(const mrpt::containers::yaml& cfg) = 0;

    virtual void fuse_pose(
        const mrpt::Clock::time_point& timestamp,
        const mrpt::poses::CPose3DPDFGaussian& pose,
        const std::string& frame_id
        ) = 0;

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

    virtual void fuse_imu(const mrpt::obs::CObservationIMU& imu) = 0;
    virtual void fuse_gnss(const mrpt::obs::CObservationGPS& gps) = 0;

    virtual void fuse_twist(
        const mrpt::Clock::time_point& timestamp,
        const mrpt::math::TTwist3D& twist,
        const mrpt::math::CMatrixDouble66& twistCov
        ) = 0;

    virtual std::optional<NavState> estimated_navstate(
        const mrpt::Clock::time_point& timestamp,
        const std::string& frame_id
        ) = 0;

Methods

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

virtual void reset()

Resets the estimator state to an initial state.

See also:

currentIntegrationState

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

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

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

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.

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

Integrates new IMU observations into the estimator

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

Integrates new GNSS observations into the estimator

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

Integrates new twist estimation (in the odom frame)

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

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