Program Listing for File BufferedImu.hpp

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

#pragma once

#include <memory>
#include <utility>

#include <navtk/aspn.hpp>
#include <navtk/filtering/containers/TimestampedDataSeries.hpp>
#include <navtk/inertial/BufferedPva.hpp>
#include <navtk/inertial/ImuErrors.hpp>
#include <navtk/inertial/Inertial.hpp>
#include <navtk/inertial/MechanizationOptions.hpp>
#include <navtk/inertial/MechanizationStandard.hpp>
#include <navtk/not_null.hpp>
#include <navtk/tensors.hpp>
#include <navtk/utils/Ordered.hpp>

namespace navtk {
namespace inertial {

class BufferedImu : public BufferedPva {

public:
    BufferedImu(const aspn_xtensor::MeasurementPositionVelocityAttitude& pva,
                std::shared_ptr<aspn_xtensor::MeasurementImu> initial_imu = nullptr,
                double expected_dt                                        = 0.01,
                const ImuErrors& imu_errs                                 = ImuErrors{},
                const MechanizationOptions& mech_options                  = MechanizationOptions{},
                double buffer_length                                      = 60.0);

    bool reset(
        std::shared_ptr<aspn_xtensor::MeasurementPositionVelocityAttitude> pva      = nullptr,
        std::shared_ptr<ImuErrors> imu_errs                                         = nullptr,
        std::shared_ptr<aspn_xtensor::MeasurementPositionVelocityAttitude> previous = nullptr);

    std::shared_ptr<aspn_xtensor::MeasurementPositionVelocityAttitude> calc_pva_no_reset_since(
        const aspn_xtensor::TypeTimestamp& time, const aspn_xtensor::TypeTimestamp& since) const;

    void add_data(not_null<std::shared_ptr<aspn_xtensor::AspnBase>> data) override;

    void mechanize(const aspn_xtensor::MeasurementImu& imu);


    void mechanize(std::shared_ptr<aspn_xtensor::MeasurementImu> imu);

    void mechanize(const aspn_xtensor::TypeTimestamp& time,
                   const Vector3& delta_v,
                   const Vector3& delta_theta);

    std::shared_ptr<aspn_xtensor::MeasurementImu> calc_force_and_rate(
        const aspn_xtensor::TypeTimestamp& time) const override;

    std::shared_ptr<aspn_xtensor::MeasurementImu> calc_force_and_rate(
        const aspn_xtensor::TypeTimestamp& time1,
        const aspn_xtensor::TypeTimestamp& time2) const override;

    std::shared_ptr<ImuErrors> get_imu_errors(const aspn_xtensor::TypeTimestamp& t) const;

private:
    // Performs actual mechanization
    Inertial ins;

    // Buffers raw inertial inputs; a sorted container where the earlier times are in the front and
    // the later times are in the back.
    filtering::TimestampedDataSeries<aspn_xtensor::MeasurementImu, aspn_xtensor::TypeTimestamp>
        imu_buf;

    // Stores IMU error parameters supplied during resets; a sorted container where the earlier
    // times are in the front and the later times are in the back.
    filtering::TimestampedDataSeries<ImuErrors, aspn_xtensor::TypeTimestamp> reset_err_buf;

    // User-provided expected delta time between IMU measurements. Used until estimated_dt() can
    // provide a usable estimate
    double expected_dt;

    // Use to calculate average in estimated_dt()
    double dt_sum;
    long num_dt;

    // Provides an estimate of the delta time between IMU measurements, calculated from
    // measurements presumed valid
    double estimated_dt() const;
};

}  // namespace inertial
}  // namespace navtk