Program Listing for File MovementDetectorPos.hpp

↰ Return to documentation for file (src/navtk/inertial/MovementDetectorPos.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>

namespace navtk {
namespace inertial {

class MovementDetectorPos : public MovementDetectorPlugin {
public:
    MovementDetectorPos(const double speed_cutoff = 0.2, const double zero_corr_distance = 100.0);

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

    aspn_xtensor::TypeTimestamp get_time() override;

private:
    /* At least one position has been received */
    bool ready_to_test = false;

    /* Last position received */
    std::shared_ptr<aspn_xtensor::MeasurementPosition> last = nullptr;

    /* Stores ctor params */
    double speed_cutoff       = 0.0;
    double zero_corr_distance = 100.0;

    /*
     * Calculate the 1 sigma NED uncertainty between pos and whatever data is stored in last.
     *
     * @param pos Most recent position measurement
     * @param est_spd Norm of NED delta position between pos and last, divided by dt (m/s)
     * @param dt Delta time between pos and last, sec.
     */
    Vector calc_sig(not_null<std::shared_ptr<aspn_xtensor::MeasurementPosition>> pos,
                    double est_spd,
                    double dt);
};
}  // namespace inertial
}  // namespace navtk