class mola::MockBiasEstimator

Overview

A provider like a state estimator would be: it averages the gyroscope readings of each IMU over windows of readings taken at rest, and models the bias as a random walk between updates.

class MockBiasEstimator:
    public mola::ExecutableBase,
    public mola::ImuBiasSource
{
public:
    // typedefs

    typedef std::function<void(const ImuBias&)> imu_bias_callback_t;
    typedef std::uint64_t subscription_id_t;

    // structs

    struct PerImu;

    // fields

    static constexpr std::size_t WINDOW_SAMPLES = 100;
    static constexpr double PRIOR_SIGMA = 0.02;
    static constexpr double PRIOR_VAR = PRIOR_SIGMA* PRIOR_SIGMA;
    static constexpr double RANDOM_WALK = 2e-5;

    // methods

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

    void add_reading(
        const std::string& label,
        const mrpt::Clock::time_point& t,
        const mrpt::math::TVector3D& w
        );

    virtual std::vector<std::string> imu_bias_sensor_labels() const;
};

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&&);
    ImuBiasSource& operator = (const ImuBiasSource&);
    ImuBiasSource& operator = (ImuBiasSource&&);
    virtual std::vector<std::string> imu_bias_sensor_labels() const = 0;

    std::optional<ImuBias> imu_bias(
        const std::string& sensorLabel,
        const std::optional<mrpt::Clock::time_point>& t = std::nullopt
        ) const;

    subscription_id_t subscribe_to_imu_bias_updates(const imu_bias_callback_t& callback);
    void unsubscribe_from_imu_bias_updates(subscription_id_t id);

Methods

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

This must be implemented to read all the required parameters

This should be reimplemented to read all the required parameters

virtual void spinOnce()

Runs any required action on a timely manner

void add_reading(
    const std::string& label,
    const mrpt::Clock::time_point& t,
    const mrpt::math::TVector3D& w
    )

Feeds one raw gyroscope reading taken at rest.

virtual std::vector<std::string> imu_bias_sensor_labels() const

Sensor labels of the IMUs with a bias estimate.