Program Listing for File MovementDetectorImu.hpp

↰ Return to documentation for file (src/navtk/inertial/MovementDetectorImu.hpp)

#pragma once

#include <memory>
#include <vector>

#include <navtk/aspn.hpp>
#include <navtk/inertial/MovementDetectorPlugin.hpp>
#include <navtk/inertial/MovementStatus.hpp>
#include <navtk/not_null.hpp>
#include <navtk/tensors.hpp>

namespace navtk {
namespace inertial {

class MovementDetectorImu : public MovementDetectorPlugin {
public:
    MovementDetectorImu(const Size window = 10, const double calib_time = 30.0);

    MovementStatus process(not_null<std::shared_ptr<aspn_xtensor::AspnBase>> data) override;

    aspn_xtensor::TypeTimestamp get_time() override;

private:
    /*
     * Empty all IMU data related buffers.
     */
    void clear_buffers();

    /* Number of measurements to consider collectively when detecting movement */
    Size window;

    /* Minimum number of seconds of stationary data assumed delivered at start */
    double calib_time;

    /* Storage for delta_theta extracted */
    std::vector<Vector3> dth_extract;

    /* Storage for time_validities extracted */
    std::vector<aspn_xtensor::TypeTimestamp> time_extract;

    /* Stationary period has passed and we can start detecting movement */
    bool ready_to_test = false;

    /* Norm of average dth from calibration period */
    double initial_dth_norm = 0.0;

    /* One-sigma uncertainty of dth norms from caibration period */
    double initial_dth_sig = 0.0;

    /* Time of last fully processed IMU measurement */
    aspn_xtensor::TypeTimestamp last_time = aspn_xtensor::TypeTimestamp((int64_t)0);
};
}  // namespace inertial
}  // namespace navtk