class mola::NavStateFilter

Overview

Virtual API for kinematic state filtering algorithms, fusing information from multiple odometry or twist sources, IMU, GNSS, etc.

Refer to the “state estimation section” in the docs

#include <NavStateFilter.h>

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

    struct GeoReferenceParams;

    // methods

    virtual void reset() = 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;

    virtual std::optional<mrpt::poses::CPose3DInterpolator> estimated_trajectory(
        ] const mrpt::Clock::time_point& start_time,
        ] const mrpt::Clock::time_point& end_time,
        ] const std::string& frame_id
        );

    void set_georeferencing_params(
        mrpt::topography::TGeodeticCoords geo_coord,
        mrpt::poses::CPose3DPDFGaussian T_enu_to_map
        );
};

Inherited Members

public:
    // methods

    virtual void initialize(const Yaml& cfg) = 0;

Methods

virtual void reset() = 0

Resets the estimator state to an initial state

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

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"
    ) = 0

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) = 0

Integrates new IMU observations into the estimator

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

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
    ) = 0

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
    ) = 0

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

virtual std::optional<mrpt::poses::CPose3DInterpolator> estimated_trajectory(
    ] const mrpt::Clock::time_point& start_time,
    ] const mrpt::Clock::time_point& end_time,
    ] const std::string& frame_id
    )

(Optional virtual method) Returns the estimated trajectory (sequence of timestamped poses) between two time points, in the given frame_id.

Returns:

std::nullopt if not implemented or not able to compute for the requested time interval.

void set_georeferencing_params(
    mrpt::topography::TGeodeticCoords geo_coord,
    mrpt::poses::CPose3DPDFGaussian T_enu_to_map
    )

Must be invoked with the mp2p_icp metric map geo-referencing information of the map in order to have GNSS observations correctly fused.