Add noise

ut-issl · Nov 7, 2023 · c8fde09 · c8fde09
1 parent 6a899e2
commit c8fde09
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 5 deletions.
diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp
@@ -7,12 +7,32 @@
 
 #include <library/initialize/initialize_file_access.hpp>
 
-OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit)
-    : Component(prescaler, clock_generator), orbit_(orbit) {}
+OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame,
+                             const libra::Vector<6> error_standard_deviation, const const Orbit& orbit)
+    : Component(prescaler, clock_generator), orbit_(orbit) {
+  for (size_t i = 0; i < 6; i++) {
+    normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i]);
+  }
+}
 
 void OrbitObserver::MainRoutine(const int time_count) {
   UNUSED(time_count);
 
+  switch (error_frame_) {
+    case ErrorFrame::kInertial:
+      observed_position_i_m_ = Measure(observed_position_i_m_);
+      // Frame conversion
+      observed_position_rtn_m_ = q_i2rtn.FrameConversion(observed_position_i_m_);
+      break;
+    case ErrorFrame::kRtn:
+      observed_position_rtn_m_ = Measure(observed_position_rtn_m_);
+      // Frame conversion
+      observed_position_i_m_ = q_i2rtn.InverseFrameConversion(observed_position_rtn_m_);
+      break;
+    default:
+      break;
+  }
+
   observed_position_i_m_ = orbit_.GetPosition_i_m();
   observed_velocity_i_m_s_ = orbit_.GetVelocity_i_m_s();
 }
@@ -36,6 +56,8 @@ std::string OrbitObserver::GetLogValue() const {
   return str_tmp;
 }
 
+void AddNoise(){}
+
 OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) {
   // General
   IniAccess ini_file(file_name);

diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp
@@ -13,6 +13,15 @@
 
 #include "../base/component.hpp"
 
+/**
+ * @enum ErrorFrame
+ * @brief Error definition frame
+ */
+enum class ErrorFrame {
+  kInertial,  //!< Inertial frame
+  kRtn,       //!< RTN frame
+};
+
 /*
  * @class OrbitObserver
  * @brief Ideal component which can observe orbit
@@ -24,9 +33,12 @@ class OrbitObserver : public Component, public ILoggable {
    * @brief Constructor without power port
    * @param [in] prescaler: Frequency scale factor for update
    * @param [in] clock_generator: Clock generator
+   * @param [in] error_frame: Error frame definition
+   * @param [in] error_standard_deviation: Position and Velocity standard deviation noise [m, m/s]
    * @param [in] orbit: Orbit information
    */
-  OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit);
+  OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame, const libra::Vector<6> error_standard_deviation,
+                const const Orbit& orbit);
 
   /**
    * @fn ~AttitudeObserver
@@ -66,11 +78,19 @@ class OrbitObserver : public Component, public ILoggable {
   inline const libra::Vector<3> GetVelocity_i_m_s() const { return observed_velocity_i_m_s_; };
 
  protected:
-  libra::Vector<3> observed_position_i_m_{0.0};    //!< Observed position @ inertial frame [m]
-  libra::Vector<3> observed_velocity_i_m_s_{0.0};  //!< Observed velocity @ inertial frame [m/s]
+  libra::Vector<3> observed_position_i_m_{0.0};      //!< Observed position @ inertial frame [m]
+  libra::Vector<3> observed_velocity_i_m_s_{0.0};    //!< Observed velocity @ inertial frame [m/s]
+  libra::Vector<3> observed_position_rtn_m_{0.0};    //!< Observed position @ RTN frame [m]
+  libra::Vector<3> observed_velocity_rtn_m_s_{0.0};  //!< Observed velocity @ RTN frame [m/s]
+
+  ErrorFrame error_frame_;                    //!< Error definition frame
+  libra::NormalRand normal_random_noise_[6];  //!< Position and Velocity noise [m, m/s]
 
   // Observed variables
   const Orbit& orbit_;  //!< Orbit information
+
+  libra::Vector<3> AddPositionNoise(const libra::Vector<3> position_m);
+  libra::Vector<3> AddVelocityNoise(const libra::Vector<3> velocity_m_s);
 };
 
 /**