Merge pull request #190 from ut-issl/hotfix/fix-sensor-base-randomize

[Hotfix] Fix sensor base randomize
ut-issl · Aug 1, 2022 · 7b6fc0d · 7b6fc0d
2 parents bca585d + 6db375d
commit 7b6fc0d
Show file tree

Hide file tree

Showing 7 changed files with 20 additions and 10 deletions.
diff --git a/src/Component/Abstract/SensorBase_tfs.hpp b/src/Component/Abstract/SensorBase_tfs.hpp
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <Library/math/GlobalRand.h>
+
 template <size_t N>
 SensorBase<N>::SensorBase(const libra::Matrix<N, N>& scale_factor, const libra::Vector<N>& range_to_const_c, const libra::Vector<N>& range_to_zero_c,
                           const libra::Vector<N>& bias_c, const libra::Vector<N>& nr_stddev_c, double rw_stepwidth,
@@ -10,7 +12,7 @@ SensorBase<N>::SensorBase(const libra::Matrix<N, N>& scale_factor, const libra::
       bias_c_(bias_c),
       n_rw_c_(rw_stepwidth, rw_stddev_c, rw_limit_c) {
   for (size_t i = 0; i < N; i++) {
-    nrs_c_[i].set_param(0.0, nr_stddev_c[i]);  // g_rand.MakeSeed()
+    nrs_c_[i].set_param(0.0, nr_stddev_c[i], g_rand.MakeSeed());
   }
   RangeCheck();
 }

diff --git a/src/Library/math/GlobalRand.h b/src/Library/math/GlobalRand.h
@@ -4,14 +4,6 @@
 
 class GlobalRand {
  public:
-  //! コンストラクタ
-  /*!
-    \param q_bd 機体座標(B)からSTT設計座標(D)への変換Quaternion
-    \param q_ds STT設計座標(D)からSTT実座標(S)への変換Quaternion
-    \param sigma_ortho 視線直交方向誤差標準偏差[rad/sec]
-    \param sigma_sight 視線方向誤差標準偏差[rad/sec]
-    \param delay STT出力遅れ。0.1秒単位範囲[0-MAX_DELAY]。
-  */
   GlobalRand();
   void SetSeed(long seed);
   long MakeSeed();

diff --git a/src/Library/math/NormalRand.hpp b/src/Library/math/NormalRand.hpp
@@ -71,6 +71,8 @@ class NormalRand {
   */
   inline void set_param(double avg, double stddev);
 
+  inline void set_param(double avg, double stddev, long seed);
+
  private:
   //! 平均値を保持するメンバ
   double avg_;

diff --git a/src/Library/math/NormalRand_ifs.hpp b/src/Library/math/NormalRand_ifs.hpp
@@ -22,6 +22,12 @@ void NormalRand::set_param(double avg, double stddev) {
   stddev_ = stddev;
 }
 
+void NormalRand::set_param(double avg, double stddev, long seed) {
+  avg_ = avg;
+  stddev_ = stddev;
+  rand_.init_seed(seed);
+}
+
 }  // namespace libra
 
 #endif  // NORMAL_RAND_IFS_HPP_
diff --git a/src/Library/math/Ran1.cpp b/src/Library/math/Ran1.cpp
@@ -11,6 +11,11 @@ Ran1::Ran1() : y_(0) { init_(); }
 
 Ran1::Ran1(long seed) : ran0_(seed), y_(0) { init_(); }
 
+void Ran1::init_seed(long seed) {
+  ran0_.init(seed);
+  init_();
+}
+
 void Ran1::init_() {
   // ran0_のウォームアップ
   for (int i = 0; i < 8; i++) {

diff --git a/src/Library/math/Ran1.hpp b/src/Library/math/Ran1.hpp
@@ -37,6 +37,8 @@ class Ran1 {
    */
   operator double();
 
+  void init_seed(long seed);
+
  private:
   //! 切り混ぜ表の初期化処理を行う関数
   void init_();

diff --git a/src/Library/math/RandomWalk_tfs.hpp b/src/Library/math/RandomWalk_tfs.hpp
@@ -1,13 +1,14 @@
 #pragma once
 
 #include <Library/utils/Macros.hpp>
+#include <Library/math/GlobalRand.h>
 
 template <size_t N>
 RandomWalk<N>::RandomWalk(double step_width, const libra::Vector<N>& stddev, const libra::Vector<N>& limit)
     : libra::ODE<N>(step_width), limit_(limit) {
   // 標準偏差設定
   for (size_t i = 0; i < N; ++i) {
-    nrs_[i].set_param(0.0, stddev[i]);  // g_rand.MakeSeed()
+    nrs_[i].set_param(0.0, stddev[i], g_rand.MakeSeed());
   }
 }