Program Listing for File crosslink_measurement.h

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

#pragma once

#include <utility>
#include <vector>

#include "lupnt/core/definitions.h"
#include "lupnt/measurements/measurement.h"
#include "lupnt/measurements/measurement_utils.h"

namespace lupnt {

  class IslCrosslinkMeasurement : public MeasurementClone<IslCrosslinkMeasurement> {
  public:
    struct Config {
      int sub_state_size = 8;
      int n_links = 0;
      int idx_position = 0;
      int idx_velocity = 3;
      int idx_clock_bias = 6;
      int idx_clock_drift = 7;

      std::vector<Vec3d> anchor_pos_mci;
      std::vector<Vec3d> anchor_vel_mci;

      double sigma_range_m = 1.0;
      double sigma_range_rate_mps = 1.0e-3;
      double sigma_pseudorange_m = 200.0;

      bool include_anchor_doppler = false;
      double sigma_anchor_doppler_mps = 1.0e-3;

      bool include_time_transfer = false;
      double sigma_time_transfer_m = 1.0;

      bool include_frequency_transfer = false;
      double sigma_frequency_transfer_mps = 1.0e-3;
    };

    IslCrosslinkMeasurement() = default;
    explicit IslCrosslinkMeasurement(Config config) : config_(std::move(config)) {}

    const Config& GetConfig() const { return config_; }
    void SetConfig(const Config& config) { config_ = config; }

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

  private:
    Config config_;

    int NumRows() const;
    VecX Model(const VecX& x) const;
    MatXd Covariance() const;
  };

}  // namespace lupnt