Program Listing for File imu.h

↰ Return to documentation for file (devices/imu.h)

#pragma once

#include "lupnt/devices/device.h"
#include "lupnt/dynamics/imu_dynamics.h"
#include "lupnt/measurements/measurement.h"
#include "lupnt/states/state.h"

namespace lupnt {

  class ImuDynamics;

  // ImuModel
  enum class ImuModel { LN200S, UNDEFINED };

  // ImuParameters
  struct ImuParameters {
    Real sigma_a;       // [m/s^2 1/sqrt(Hz))] Accelerometer white noise
    Real sigma_a_bias;  // [m/s^2 sqrt(Hz))] Accelerometer bias random walk
    Real sigma_w;       // [rad/s 1/sqrt(Hz))] Gyroscope white noise
    Real sigma_w_bias;  // [rad/s sqrt(Hz))] Gyroscope bias random walk
  };

  class Imu : public Device {
  protected:
    ImuModel model_;
    Real time_ = 0.0;  // [s] Time
    ImuState state_;

    std::vector<ImuMeasurement> data_;
    Ptr<ImuDynamics> dynamics_;

    Vec3 position_ = Vec3::Zero();         // [m] Position of the IMU in the body frame
    Mat3 orientation_ = Mat3::Identity();  // Orientation of the IMU in the body frame

  public:
    Imu();
    Imu(Config& config);

    void Step(Real t) override;
    void Setup() override;

    ImuMeasurement GetMeasurement(Real t);

    static ImuParameters GetParameters(ImuModel imu_model);

    std::vector<ImuMeasurement> GetData() const { return data_; }
    void EmptyData() { data_.clear(); }

    void Log(Real time) override;
  };

}  // namespace lupnt