Program Listing for File measurement.h

↰ Return to documentation for file (measurements/measurement.h)

#pragma once

#include "lupnt/core/definitions.h"
#include "lupnt/numerics/filters/filter.h"
#include "lupnt/states/state.h"

namespace lupnt {

  struct MeasData {
    Real timestamp = 0.0;
    VecXd value;
    MatXd covariance;
  };

  struct ImuMeasurement {
    Real timestamp = 0.0;
    Vec3 w = Vec3::Zero();
    Vec3 a = Vec3::Zero();
  };

  class Measurement {
  public:
    virtual ~Measurement() = default;

    virtual MeasData Compute(const State& x, MatXd* H = nullptr) const = 0;

    virtual Ptr<Measurement> Clone() const = 0;

    virtual FilterMeasurementFunction CreateFunction() const {
      Ptr<Measurement> self = Clone();
      return [self](const State& x, MatXd* H, MatXd* R) -> VecXd {
        MeasData md = self->Compute(x, H);
        if (R != nullptr) *R = md.covariance;
        return md.value;
      };
    }
  };

  template <typename Derived> class MeasurementClone : public Measurement {
  public:
    Ptr<Measurement> Clone() const override {
      return MakePtr<Derived>(static_cast<const Derived&>(*this));
    }
  };

  struct NavErrorContext {
    Vec3d r = Vec3d::Zero();
    Vec3d v = Vec3d::Zero();
    Mat3d R_b2n = Mat3d::Identity();
    double clock_bias_s = 0.0;
    double clock_drift_sps = 0.0;
    int error_state_size = 0;
  };

  class ErrorStateMeasurement {
  public:
    virtual ~ErrorStateMeasurement() = default;

    virtual MeasData Compute(const NavErrorContext& nominal, MatXd* H = nullptr) const = 0;
  };

}  // namespace lupnt