class mola::GravityLeveler

Overview

Causal gravity leveling of a trajectory produced by an odometry or a state estimator, without touching the estimator itself.

The trajectory’s frame may slowly tilt with respect to gravity (e.g. where the scans barely constrain pitch and roll), which turns horizontal motion into a vertical drift. This class estimates that tilt and rotates the trajectory increments to undo it:

Measurement, every epoch_period seconds: the mean specific force over a trailing window, each reading rotated into the trajectory frame with the trajectory’s attitude at its own time, minus the trajectory’s own mean acceleration over the same interval. Normalized, its horizontal part is the frame tilt tau (2 DoF, small angles), plus the effect of any body-fixed accelerometer error.
Readings beyond max_tilt, or with gravity not pointing up, are ignored: a frame tilt is small, and a large reading means the trajectory’s attitude itself is failing.
A Kalman filter with state [tau (2), b (3)] : tau a random walk with tilt_random_walk deg/sqrt(s), b a constant body-frame accelerometer error (bias plus IMU misalignment), observed through the mean attitude over the window. A body-fixed error turns with the platform and a frame tilt does not, which separates both while the platform turns. Readings beyond a chi-squared gate are ignored; after max_consecutive_rejections of them in a row the filter restarts from the current reading, so a wrong state cannot lock itself in.
Output: the part of the estimated tilt beyond deadband degrees is removed by rotating every increment of the trajectory, and its attitude, by the small rotation that levels the frame. The deadband leaves alone a frame that is already level within the reading’s own error.

Only past data are used. Positions are corrected incrementally, so the output is continuous even when the correction changes.

#include <GravityLeveler.h>

class GravityLeveler
{
public:
    // structs

    struct ImuSample;
    struct Parameters;
    struct PoseSample;
    struct Stats;

    // construction

    GravityLeveler();
    GravityLeveler(const Parameters& p);

    // methods

    const Parameters& parameters() const;
    const Stats& stats() const;
    void add_imu(double t, const mrpt::math::TVector3D& specific_force);
    mrpt::poses::CPose3D add_pose(double t, const mrpt::poses::CPose3D& pose);
    std::optional<mrpt::math::TVector2D> tilt() const;
    mrpt::math::TVector2D applied_tilt() const;
};

Construction

GravityLeveler(const Parameters& p)

Parameters:

std::exception

on invalid parameters

Methods

void add_imu(double t, const mrpt::math::TVector3D& specific_force)

Adds one accelerometer reading (specific force, in the vehicle frame)

mrpt::poses::CPose3D add_pose(double t, const mrpt::poses::CPose3D& pose)

Adds the next pose of the trajectory and returns it leveled. IMU readings up to t should have been added before. A pose not newer than the previous one is leveled with the current correction but does not update the filter.

std::optional<mrpt::math::TVector2D> tilt() const

The current tilt estimate (x, y components, in radians), before the deadband. Empty until the first reading has been taken.

mrpt::math::TVector2D applied_tilt() const

The tilt being removed from the output (after the deadband) [rad]