Program Listing for File Inertial.hpp

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

#pragma once

#include <memory>

#include <navtk/inertial/AidingAltData.hpp>
#include <navtk/inertial/ImuErrors.hpp>
#include <navtk/inertial/InertialPosVelAtt.hpp>
#include <navtk/inertial/Mechanization.hpp>
#include <navtk/inertial/MechanizationOptions.hpp>
#include <navtk/inertial/MechanizationStandard.hpp>
#include <navtk/inertial/StandardPosVelAtt.hpp>
#include <navtk/inertial/WanderPosVelAtt.hpp>
#include <navtk/inertial/mechanization_standard.hpp>
#include <navtk/not_null.hpp>
#include <navtk/tensors.hpp>

namespace navtk {
namespace inertial {

using MechanizationFunction = std::function<not_null<std::shared_ptr<InertialPosVelAtt>>(
    const Vector3& dv,
    const Vector3& dth,
    const double dt,
    const not_null<std::shared_ptr<InertialPosVelAtt>> ref,
    const not_null<std::shared_ptr<InertialPosVelAtt>> old_ref,
    const MechanizationOptions& options,
    AidingAltData* aiding_alt_data)>;

class Inertial final {

public:
    explicit Inertial(const not_null<std::shared_ptr<InertialPosVelAtt>> start_pva =
                          std::make_shared<StandardPosVelAtt>(),
                      const MechanizationOptions& mech_options = MechanizationOptions{},
                      MechanizationFunction mech_fun =
                          static_cast<not_null<std::shared_ptr<InertialPosVelAtt>> (*)(
                              const Vector3&,
                              const Vector3&,
                              const double,
                              const not_null<std::shared_ptr<InertialPosVelAtt>>,
                              const not_null<std::shared_ptr<InertialPosVelAtt>>,
                              const MechanizationOptions&,
                              AidingAltData* aiding_alt_data)>(mechanization_standard));

    explicit Inertial(not_null<std::shared_ptr<Mechanization>> mech_class,
                      const not_null<std::shared_ptr<InertialPosVelAtt>> start_pva =
                          std::make_shared<StandardPosVelAtt>(),
                      const MechanizationOptions& mech_options = MechanizationOptions{});

    Inertial(const Inertial& ins);

    Inertial& operator=(const Inertial& ins);

    Inertial(Inertial&&) = default;

    Inertial& operator=(Inertial&&) = default;

    ~Inertial() noexcept = default;

    void reset(const not_null<std::shared_ptr<InertialPosVelAtt>> new_pva,
               const std::shared_ptr<InertialPosVelAtt> old_pva = nullptr);

    void reset(StandardPosVelAtt new_pva);

    void reset(WanderPosVelAtt new_pva);

    not_null<std::shared_ptr<InertialPosVelAtt>> get_solution() const;

    Vector3 get_gyro_biases() const;

    void set_gyro_biases(Vector3 gyro_biases);

    Vector3 get_gyro_scale_factors() const;

    void set_gyro_scale_factors(Vector3 gyro_scale_factors);

    Vector3 get_accel_biases() const;

    void set_accel_biases(Vector3 accel_biases);

    Vector3 get_accel_scale_factors() const;

    void set_accel_scale_factors(Vector3 accel_scale_factors);

    void set_imu_errors(const ImuErrors& errors);

    void mechanize(const aspn_xtensor::TypeTimestamp& time,
                   const Vector3& accel_meas,
                   const Vector3& gyro_meas,
                   AidingAltData* aiding_alt_data = nullptr);
    double integrated_alt_error;

private:
    not_null<std::shared_ptr<InertialPosVelAtt>> pva;
    not_null<std::shared_ptr<InertialPosVelAtt>> pva_old1;
    Vector3 gyro_bias;
    Vector3 gyro_scale_factor;
    Vector3 accel_bias;
    Vector3 accel_scale_factor;
    MechanizationFunction _mech_fun;
    MechanizationOptions _mech_options;

    void apply_error_model(double dt);
    Vector3 dv_cpy;
    Vector3 dth_cpy;
};

}  // namespace inertial
}  // namespace navtk