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 mola::RawDataConsumer { public: // methods virtual void reset() = 0; virtual void initialize(const Yaml& cfg); 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 ); virtual bool has_converged_localization(] mrpt::poses::CPose3DPDFGaussian& pose_in_map) const; virtual void set_geo_reference(] const mola::Georeferencing& georef); virtual std::optional<mola::Georeferencing> get_geo_reference() const; virtual bool transform_frame(] const mrpt::poses::CPose3D& b); virtual void fuse_pose_deferred( const mrpt::Clock::time_point& timestamp, const mrpt::poses::CPose3D& mean, const std::shared_future<mrpt::math::CMatrixDouble66>& cov, const std::string& frame_id ); virtual void reset_motion(); }; // direct descendants class StateEstimationSimple; class StateEstimationSmoother;
Inherited Members¶
public: // structs struct DiagnosticsOutput; // methods virtual void initialize(const Yaml& cfg) = 0; virtual void spinOnce() = 0; ExecutableBase& operator = (const ExecutableBase&); ExecutableBase& operator = (ExecutableBase&&);
Methods¶
virtual void reset() = 0
Resets the estimator state to an initial state
virtual void initialize(const Yaml& cfg)
initialize() : loads “raw_data_source”. If reimplemented in a derived class, remember to call the base class.
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 a given 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, given in the local vehicle frame like NavState::twist, with twistCov in the order [vx vy vz wx wy wz].
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.
virtual bool has_converged_localization(] mrpt::poses::CPose3DPDFGaussian& pose_in_map) const
(Optional virtual method) Returns true if the estimator has converged to a stable state with uncertainty below reasonable thresholds.
Parameters:
pose |
The current estimated pose with covariance (in the “map” frame_id) |
Returns:
true if converged, false otherwise or if not implemented.
virtual void set_geo_reference(] const mola::Georeferencing& georef)
(Optional virtual method) Provides the estimator with the geo-reference (datum + T_enu_to_map) of the map it is localizing against, so that GNSS observations can be converted into map-frame constraints. Typically called by the localization front-end once the georeferenced map is loaded. The default implementation ignores it (estimators without GNSS support).
virtual std::optional<mola::Georeferencing> get_geo_reference() const
(Optional virtual method) Returns the geo-reference currently in use by the estimator, or std::nullopt if none has been set / not supported.
virtual bool transform_frame(] const mrpt::poses::CPose3D& b)
(Optional virtual method) Re-expresses the whole estimator state in a new reference frame: every stored map-frame pose p must become b + p.
Intended for a one-off gauge change of the map frame (e.g. leveling it once gravity is known), NOT for a state update: relative motion, and hence every velocity expressed in the vehicle’s own frame, is unchanged by construction.
Declared last on purpose: appending a virtual only grows the vtable, whereas inserting one shifts the slot index of every virtual after it. Keep new optional virtuals here, at the end.
Returns:
true if applied, false if this estimator cannot do it (the default). A caller that gets false must fall back to reset().
virtual void fuse_pose_deferred( const mrpt::Clock::time_point& timestamp, const mrpt::poses::CPose3D& mean, const std::shared_future<mrpt::math::CMatrixDouble66>& cov, const std::string& frame_id )
(Optional virtual method) Like fuse_pose(), for a pose whose covariance is still being computed elsewhere (e.g. by a worker thread of the front end), so the caller does not have to wait for it. cov becomes ready later, in the parameterization of mrpt::poses::CPose3DPDFGaussian::cov.
Estimators that process measurements asynchronously should override this to wait for cov off the caller’s thread. The default waits for it here and calls fuse_pose().
virtual void reset_motion()
(Optional virtual method) Forgets the motion state (velocities and recent poses; estimator-tracked sensor biases may be kept, as with reset()), e.g. after its prediction proved implausible, but keeps the reference frames: {map}, each odometry frame, the gravity (ENU) alignment and the geo-reference datum. The poses fused next continue in the same frames as before. reset(), instead, starts from scratch: the next poses may define the frames again, as needed after a re-localization.
The default calls reset(). An estimator whose reset() redefines frames (e.g. anchors {map} to the next pose, or levels gravity again) must override it to keep them. Inputs still pending at the call may be dropped. Callable from any thread, like reset() and the fuse_*() methods.