Program Listing for File DynData.hpp

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

#pragma once

#include <vector>

#include <navtk/aspn.hpp>
#include <navtk/factory.hpp>
#include <navtk/tensors.hpp>

namespace navtk {
namespace inertial {

class DynData {
public:
    enum class RecentPositionsEnum {
        MOST_RECENT,
        SECOND_MOST_RECENT,
        THIRD_MOST_RECENT,
    };

    DynData(const aspn_xtensor::MeasurementPosition& origin);

    bool enough_data();

    void update(const aspn_xtensor::MeasurementPosition& new_pos,
                const std::vector<aspn_xtensor::MeasurementImu>& align_buffer);

    std::pair<bool, Vector3> get_force_from_imu();

    Vector3 get_force_from_pos();

    std::pair<aspn_xtensor::TypeTimestamp, Vector3> get_vel_mid();

    [[deprecated(
        "Deprecated due to extra copies. Use get_position instead which returns one "
        "position ")]] std::vector<aspn_xtensor::MeasurementPosition>
    get_positions();


    const aspn_xtensor::MeasurementPosition& get_position(const RecentPositionsEnum& recency) const;

    const aspn_xtensor::MeasurementPosition& get_origin() const;

    std::pair<double, double> get_lat_lon_factors();

private:
    /* First position supplied, forms the origin of the local NED frame */
    aspn_xtensor::MeasurementPosition origin;
    /* Multiply by delta latitude in radians to get meters north in local level frame at origin */
    double lat_factor = 0.0;
    /* Multiply by delta longitude in radians to get meters east in local level frame at origin */
    double lon_factor = 0.0;
    /* The most recent supplied position, time t(k)*/
    aspn_xtensor::MeasurementPosition current;
    /* The last position passed in, time t(k-1) */
    aspn_xtensor::MeasurementPosition prior;
    /* The position supplied 2 calls ago, time t(k-2) */
    aspn_xtensor::MeasurementPosition prior2;
    /* The avg NED velocity between times t(k-1) and t(k) (m/s) */
    Vector3 vel_current_prior = zeros(3);
    /* The avg NED velocity between times t(k-2) and t(k-1) (m/s) */
    Vector3 vel_prior_prior2 = zeros(3);
    /* The avg NED velocity at time t(k-1) (avg of vel_current_prior and vel_prior_prior2) (m/s) */
    Vector3 vel_prior = zeros(3);
    /* Time halfway between t(k-1) and t(k) (i.e. time of validity for vel_current_prior) */
    aspn_xtensor::TypeTimestamp t_current_prior;
    /* Time halfway between t(k-2) and t(k-1) (i.e. time of validity for vel_prior_prior2) */
    aspn_xtensor::TypeTimestamp t_prior_prior2;
    /* The respective averages of the first and second halves of the IMU data measured over t(k-2)
     * (exclusive) to t(k-1) (inclusive) */
    std::pair<Vector3, Vector3> split_forces_prior_prior2;
    /* True if split_forces_prior_prior2 is suitable for use in force calculations */
    bool prior_prior2_imu_valid = false;
    /* The respective averages of the first and second halves of the IMU data measured over t(k-1)
     * (exclusive) to t(k) (inclusive) */
    std::pair<Vector3, Vector3> split_forces_current_prior;
    /* True if split_forces_current_prior is suitable for use in force calculations */
    bool current_prior_imu_valid = false;
    /* Average specific force at t(k-1), from IMU data over t(k-1.5) to t(k-0.5) */
    Vector3 avg_force_prior_imu = zeros(3);
    /* Estimated specific force at t(k-1), from double differencing pos data over t(k-2) to t(k) */
    Vector3 avg_force_prior_pos = zeros(3);
    /* Tracks number of times update called to determine when sufficient data is available to begin
     * calculating forces. */
    Size update_counts = 0;

    /*
     * Calculate the NED delta position from origin.
     *
     * @param p Position to place in local level frame.
     * @return NED position in local frame, meters NED.
     */
    Vector3 delta_pos(const aspn_xtensor::MeasurementPosition& p) const;

    /*
     * Calculate estimated specific force from velocity measurements.
     *
     * @param pos Position at which to calculate the forces (pos halfway between t_v0 and t_v1).
     * @param v0 NED velocity at t_v0 (m/s).
     * @param v1 NED velocity at t_v1 (m/s).
     * @param t_v0 Time of validity for v0.
     * @param t_v1 Time of validity for v1.
     *
     * @return Estimated NED specific force in m/s^2 at time halfway between t_v0 and t_v1.
     */
    Vector3 force_from_velocity(const aspn_xtensor::MeasurementPosition& pos,
                                const Vector3& v0,
                                const Vector3& v1,
                                const aspn_xtensor::TypeTimestamp& t_v0,
                                const aspn_xtensor::TypeTimestamp& t_v1);
};
}  // namespace inertial
}  // namespace navtk