From 310493986e692b5529d9dc7a8dc91a1fc703240e Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 3 Nov 2023 20:39:39 +0100 Subject: [PATCH 01/17] Add orbit observer --- src/components/ideal/orbit_observer.hpp | 78 +++++++++++++++++++++++++ 1 file changed, 78 insertions(+) create mode 100644 src/components/ideal/orbit_observer.hpp diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp new file mode 100644 index 000000000..97b9216e0 --- /dev/null +++ b/src/components/ideal/orbit_observer.hpp @@ -0,0 +1,78 @@ +/* + * @file orbit_observer.hpp + * @brief Ideal component which can observe orbit + */ + +#ifndef S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_ +#define S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_ + +#include +#include +#include +#include + +#include "../base/component.hpp" + +/* + * @class OrbitObserver + * @brief Ideal component which can observe orbit + */ +class OrbitObserver : public Component, public ILoggable { + public: + /** + * @fn OrbitObserver + * @brief Constructor without power port + * @param [in] prescaler: Frequency scale factor for update + * @param [in] clock_generator: Clock generator + * @param [in] orbit: Orbit information + */ + OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit* orbit); + + /** + * @fn ~AttitudeObserver + * @brief Destructor + */ + ~OrbitObserver() {} + + // Override functions for Component + /** + * @fn MainRoutine + * @brief Main routine for sensor observation + */ + void MainRoutine(const int time_count) override; + + // Override ILoggable + /** + * @fn GetLogHeader + * @brief Override GetLogHeader function of ILoggable + */ + virtual std::string GetLogHeader() const override; + /** + * @fn GetLogValue + * @brief Override GetLogValue function of ILoggable + */ + virtual std::string GetLogValue() const override; + + /** + * @fn GetPosition_i_m + * @brief Return observed quaternion from the inertial frame to the component frame + */ + inline const libra::Vector<3> GetPosition() const { return observed_position_i_m_; }; + + protected: + libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] + + // Observed variables + const Orbit* orbit_; //!< Orbit information +}; + +/** + * @fn InitStarSensor + * @brief Initialize functions for StarSensor without power port + * @param [in] clock_generator: Clock generator + * @param [in] file_name: Path to the initialize file + * @param [in] orbit: Orbit information + */ +OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit* orbit); + +#endif // S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_ From d174ef74b4635d5f3c5090e450eb5728dd12a818 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 4 Nov 2023 08:25:25 +0100 Subject: [PATCH 02/17] Add cpp file --- src/components/ideal/orbit_observer.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 src/components/ideal/orbit_observer.cpp diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp new file mode 100644 index 000000000..ec6b4bec7 --- /dev/null +++ b/src/components/ideal/orbit_observer.cpp @@ -0,0 +1,8 @@ +/* + * @file orbit_observer.cpp + * @brief Ideal component which can observe orbit + */ + +#include "orbit_observer.hpp" + +OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit* orbit) : Component(prescaler, clock_generator), orbit_(orbit) {} From 3fc4c0abfb5acc6d3b47a289521a0d93cc72ecf1 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 4 Nov 2023 14:22:58 +0100 Subject: [PATCH 03/17] Implement main routine without noise --- src/components/ideal/orbit_observer.cpp | 38 ++++++++++++++++++++++++- src/components/ideal/orbit_observer.hpp | 4 +-- 2 files changed, 39 insertions(+), 3 deletions(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index ec6b4bec7..6d1171b89 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -5,4 +5,40 @@ #include "orbit_observer.hpp" -OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit* orbit) : Component(prescaler, clock_generator), orbit_(orbit) {} +OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit) : Component(prescaler, clock_generator), orbit_(orbit) {} + +void OrbitObserver::MainRoutine(const int time_count) { + UNUSED(time_count); + + observed_position_i_m_ = orbit_.GetPosition_i_m(); +} + +std::string OrbitObserver::GetLogHeader() const { + std::string str_tmp = ""; + + std::string head = "orbit_observer_"; + str_tmp += WriteVector(head + "position", "i", "m", 3); + + return str_tmp; +} + +std::string OrbitObserver::GetLogValue() const { + std::string str_tmp = ""; + + str_tmp += WriteVector(observed_position_i_m_, 16); + + return str_tmp; +} + +OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) { + // General + IniAccess ini_file(file_name); + + // CompoBase + int prescaler = ini_file.ReadInt("COMPONENT_BASE", "prescaler"); + if (prescaler <= 1) prescaler = 1; + + OrbitObserver orbit_observer(prescaler, clock_generator, orbit); + + return orbit_observer; +} diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 97b9216e0..c495a06d1 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -26,7 +26,7 @@ class OrbitObserver : public Component, public ILoggable { * @param [in] clock_generator: Clock generator * @param [in] orbit: Orbit information */ - OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit* orbit); + OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit); /** * @fn ~AttitudeObserver @@ -73,6 +73,6 @@ class OrbitObserver : public Component, public ILoggable { * @param [in] file_name: Path to the initialize file * @param [in] orbit: Orbit information */ -OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit* orbit); +OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit); #endif // S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_ From 4b38b432ba779117adb7531648955ce18363553c Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 4 Nov 2023 14:25:57 +0100 Subject: [PATCH 04/17] dd to CMake --- src/components/CMakeLists.txt | 1 + src/components/ideal/orbit_observer.cpp | 4 +++- src/components/ideal/orbit_observer.hpp | 2 +- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/components/CMakeLists.txt b/src/components/CMakeLists.txt index 697b2322f..2cc69c93d 100644 --- a/src/components/CMakeLists.txt +++ b/src/components/CMakeLists.txt @@ -37,6 +37,7 @@ ideal/force_generator.cpp ideal/torque_generator.cpp ideal/angular_velocity_observer.cpp ideal/attitude_observer.cpp +ideal/orbit_observer.cpp real/mission/telescope.cpp diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 6d1171b89..f66e9802f 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -5,7 +5,9 @@ #include "orbit_observer.hpp" -OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit) : Component(prescaler, clock_generator), orbit_(orbit) {} +#include + +OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit) : Component(prescaler, clock_generator), orbit_(orbit) {} void OrbitObserver::MainRoutine(const int time_count) { UNUSED(time_count); diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index c495a06d1..5990e5f89 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -63,7 +63,7 @@ class OrbitObserver : public Component, public ILoggable { libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] // Observed variables - const Orbit* orbit_; //!< Orbit information + const Orbit& orbit_; //!< Orbit information }; /** From 6975859ba4dbbf40aa8d950cce9e6be2da7a2289 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 4 Nov 2023 14:36:02 +0100 Subject: [PATCH 05/17] Add velocity --- src/components/ideal/orbit_observer.cpp | 6 +++++- src/components/ideal/orbit_observer.hpp | 3 ++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index f66e9802f..38cdeed6c 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -7,12 +7,14 @@ #include -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 Orbit& orbit) + : Component(prescaler, clock_generator), orbit_(orbit) {} void OrbitObserver::MainRoutine(const int time_count) { UNUSED(time_count); observed_position_i_m_ = orbit_.GetPosition_i_m(); + observed_velocity_i_m_s_ = orbit_.GetVelocity_i_m_s(); } std::string OrbitObserver::GetLogHeader() const { @@ -20,6 +22,7 @@ std::string OrbitObserver::GetLogHeader() const { std::string head = "orbit_observer_"; str_tmp += WriteVector(head + "position", "i", "m", 3); + str_tmp += WriteVector(head + "velocity", "i", "m/s", 3); return str_tmp; } @@ -28,6 +31,7 @@ std::string OrbitObserver::GetLogValue() const { std::string str_tmp = ""; str_tmp += WriteVector(observed_position_i_m_, 16); + str_tmp += WriteVector(observed_velocity_i_m_s_, 16); return str_tmp; } diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 5990e5f89..78bfcfdd3 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -60,7 +60,8 @@ class OrbitObserver : public Component, public ILoggable { inline const libra::Vector<3> GetPosition() const { return observed_position_i_m_; }; protected: - libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] + 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] // Observed variables const Orbit& orbit_; //!< Orbit information From fa55700acae12e78d464371ef5bdae5da22e94fe Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sat, 4 Nov 2023 14:36:46 +0100 Subject: [PATCH 06/17] Add velocity --- src/components/ideal/orbit_observer.hpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 78bfcfdd3..e2bbba1ef 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -55,9 +55,15 @@ class OrbitObserver : public Component, public ILoggable { /** * @fn GetPosition_i_m - * @brief Return observed quaternion from the inertial frame to the component frame + * @brief Return observed position */ - inline const libra::Vector<3> GetPosition() const { return observed_position_i_m_; }; + inline const libra::Vector<3> GetPosition_i_m() const { return observed_position_i_m_; }; + + /** + * @fn GetVelocity_i_m_s + * @brief Return observed velocity + */ + 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] From b8019e5d3ad93902a2d3e93208a377195d7ef522 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 7 Nov 2023 08:09:20 +0100 Subject: [PATCH 07/17] Add noise --- src/components/ideal/orbit_observer.cpp | 26 +++++++++++++++++++++++-- src/components/ideal/orbit_observer.hpp | 26 ++++++++++++++++++++++--- 2 files changed, 47 insertions(+), 5 deletions(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 38cdeed6c..d065ebac4 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -7,12 +7,32 @@ #include -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 index e2bbba1ef..2271ae271 100644 --- 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); }; /** From 5f00035ca7b64eb168f01b2f46630384af1b505c Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 15:23:34 +0100 Subject: [PATCH 08/17] Fix format --- src/components/ideal/orbit_observer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index d065ebac4..ba12a9b2f 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -56,7 +56,7 @@ std::string OrbitObserver::GetLogValue() const { return str_tmp; } -void AddNoise(){} +void AddNoise() {} OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) { // General From 475d4c42a9cfc8f461eecc25dce9654d90e31003 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 15:33:36 +0100 Subject: [PATCH 09/17] Fix for build success --- src/components/ideal/orbit_observer.cpp | 18 ++++++++++-------- src/components/ideal/orbit_observer.hpp | 2 +- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index ba12a9b2f..053941c49 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -8,7 +8,7 @@ #include OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame, - const libra::Vector<6> error_standard_deviation, const const Orbit& orbit) + const libra::Vector<6> error_standard_deviation, 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]); @@ -18,23 +18,21 @@ OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generato void OrbitObserver::MainRoutine(const int time_count) { UNUSED(time_count); + libra::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh(); switch (error_frame_) { case ErrorFrame::kInertial: - observed_position_i_m_ = Measure(observed_position_i_m_); + observed_position_i_m_ = AddPositionNoise(orbit_.GetPosition_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_rtn_m_ = AddPositionNoise(orbit_.GetPosition_); + // 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(); } std::string OrbitObserver::GetLogHeader() const { @@ -66,7 +64,11 @@ OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std int prescaler = ini_file.ReadInt("COMPONENT_BASE", "prescaler"); if (prescaler <= 1) prescaler = 1; - OrbitObserver orbit_observer(prescaler, clock_generator, orbit); + // Noise + const ErrorFrame error_frame = ErrorFrame::kInertial; + const libra::Vector<6> error_standard_deviation; + + OrbitObserver orbit_observer(prescaler, clock_generator, error_frame, error_standard_deviation, orbit); return orbit_observer; } diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 2271ae271..9f0d79314 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -38,7 +38,7 @@ class OrbitObserver : public Component, public ILoggable { * @param [in] orbit: Orbit information */ OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame, const libra::Vector<6> error_standard_deviation, - const const Orbit& orbit); + const Orbit& orbit); /** * @fn ~AttitudeObserver From d9e76b007c807aa96b417ebcf77351f926af43ac Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 15:46:10 +0100 Subject: [PATCH 10/17] Fix noise calculation --- src/components/ideal/orbit_observer.cpp | 23 +++++++++++++++-------- src/components/ideal/orbit_observer.hpp | 10 ++++------ 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 053941c49..4ee50551b 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -6,33 +6,42 @@ #include "orbit_observer.hpp" #include +#include OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame, const libra::Vector<6> error_standard_deviation, 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]); + normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], global_randomization.MakeSeed()); } } void OrbitObserver::MainRoutine(const int time_count) { UNUSED(time_count); + // Calc noise + libra::Vector<3> position_error_i_m{0.0}; + libra::Vector<3> position_error_rtn_m{0.0}; libra::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh(); switch (error_frame_) { case ErrorFrame::kInertial: - observed_position_i_m_ = AddPositionNoise(orbit_.GetPosition_i_m()); - // Frame conversion - observed_position_rtn_m_ = q_i2rtn.FrameConversion(observed_position_i_m_); + for (size_t axis = 0; axis < 3; axis++) { + position_error_i_m[axis] = normal_random_noise_[axis]; + } break; case ErrorFrame::kRtn: - // observed_position_rtn_m_ = AddPositionNoise(orbit_.GetPosition_); + for (size_t axis = 0; axis < 3; axis++) { + position_error_rtn_m[axis] = normal_random_noise_[axis]; + } // Frame conversion - observed_position_i_m_ = q_i2rtn.InverseFrameConversion(observed_position_rtn_m_); + position_error_i_m = q_i2rtn.InverseFrameConversion(position_error_rtn_m); break; default: break; } + + // Get observed value + observed_position_i_m_ = orbit_.GetPosition_i_m() + position_error_i_m; } std::string OrbitObserver::GetLogHeader() const { @@ -54,8 +63,6 @@ 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 index 9f0d79314..0481263e2 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -78,10 +78,8 @@ 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_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] + 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] ErrorFrame error_frame_; //!< Error definition frame libra::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s] @@ -89,8 +87,8 @@ class OrbitObserver : public Component, public ILoggable { // 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); + libra::Vector<3> CalcPositionNoise(); + libra::Vector<3> CalcVelocityNoise(); }; /** From 000c5c1260fc45591246f47c27f6d2875f3a3cf0 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 15:48:09 +0100 Subject: [PATCH 11/17] Fix small --- src/components/ideal/orbit_observer.cpp | 2 +- src/components/ideal/orbit_observer.hpp | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 4ee50551b..1b7283eca 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -10,7 +10,7 @@ OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame, const libra::Vector<6> error_standard_deviation, const Orbit& orbit) - : Component(prescaler, clock_generator), orbit_(orbit) { + : Component(prescaler, clock_generator), error_frame_(error_frame), orbit_(orbit) { for (size_t i = 0; i < 6; i++) { normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], global_randomization.MakeSeed()); } diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 0481263e2..03963afe9 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -86,9 +86,6 @@ class OrbitObserver : public Component, public ILoggable { // Observed variables const Orbit& orbit_; //!< Orbit information - - libra::Vector<3> CalcPositionNoise(); - libra::Vector<3> CalcVelocityNoise(); }; /** From fa9d18d406e4a77d37049779cd60b76f3946fbe3 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 16:00:38 +0100 Subject: [PATCH 12/17] Add initialize file --- .../components/orbit_observer.ini | 19 ++++++++++++ src/components/ideal/orbit_observer.cpp | 29 ++++++++++++++----- src/components/ideal/orbit_observer.hpp | 20 +++++++++---- 3 files changed, 54 insertions(+), 14 deletions(-) create mode 100644 data/sample/initialize_files/components/orbit_observer.ini diff --git a/data/sample/initialize_files/components/orbit_observer.ini b/data/sample/initialize_files/components/orbit_observer.ini new file mode 100644 index 000000000..8d527e618 --- /dev/null +++ b/data/sample/initialize_files/components/orbit_observer.ini @@ -0,0 +1,19 @@ +[ORBIT_OBSERVER] +// Noise definition frame +// INERTIAL: Inertial frame +// RTN: RTN frame +noise_frame = INERTIAL + +// Standard deviation of position and velocity noise [m, m/s] +// The frame definition can be selected above +noise_standard_deviation[0] = 1000 // Position-X +noise_standard_deviation[1] = 2000 // Position-Y +noise_standard_deviation[2] = 3000 // Position-Z +noise_standard_deviation[3] = 30 // Velocity-X +noise_standard_deviation[4] = 20 // Velocity-Y +noise_standard_deviation[5] = 10 // Velocity-Z + + +[COMPONENT_BASE] +// Prescaler with respect to the component update period +prescaler = 1 diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 1b7283eca..26d816933 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -8,9 +8,9 @@ #include #include -OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame, +OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame, const libra::Vector<6> error_standard_deviation, const Orbit& orbit) - : Component(prescaler, clock_generator), error_frame_(error_frame), orbit_(orbit) { + : Component(prescaler, clock_generator), noise_frame_(noise_frame), orbit_(orbit) { for (size_t i = 0; i < 6; i++) { normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], global_randomization.MakeSeed()); } @@ -23,13 +23,13 @@ void OrbitObserver::MainRoutine(const int time_count) { libra::Vector<3> position_error_i_m{0.0}; libra::Vector<3> position_error_rtn_m{0.0}; libra::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh(); - switch (error_frame_) { - case ErrorFrame::kInertial: + switch (noise_frame_) { + case NoiseFrame::kInertial: for (size_t axis = 0; axis < 3; axis++) { position_error_i_m[axis] = normal_random_noise_[axis]; } break; - case ErrorFrame::kRtn: + case NoiseFrame::kRtn: for (size_t axis = 0; axis < 3; axis++) { position_error_rtn_m[axis] = normal_random_noise_[axis]; } @@ -63,6 +63,18 @@ std::string OrbitObserver::GetLogValue() const { return str_tmp; } +NoiseFrame SetNoiseFrame(const std::string noise_frame) { + if (noise_frame == "INERTIAL") { + return NoiseFrame::kInertial; + } else if (noise_frame == "RTN") { + return NoiseFrame::kRtn; + } else { + std::cerr << "[WARNINGS] Orbit observer noise frame is not defined!" << std::endl; + std::cerr << "The noise frame is automatically initialized as INERTIAL" << std::endl; + return NoiseFrame::kInertial; + } +} + OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) { // General IniAccess ini_file(file_name); @@ -72,10 +84,11 @@ OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std if (prescaler <= 1) prescaler = 1; // Noise - const ErrorFrame error_frame = ErrorFrame::kInertial; - const libra::Vector<6> error_standard_deviation; + const NoiseFrame noise_frame = SetNoiseFrame(ini_file.ReadString("ORBIT_OBSERVER", "noise_frame")); + libra::Vector<6> noise_standard_deviation; + ini_file.ReadVector("ORBIT_OBSERVER", "noise_standard_deviation", noise_standard_deviation); - OrbitObserver orbit_observer(prescaler, clock_generator, error_frame, error_standard_deviation, orbit); + OrbitObserver orbit_observer(prescaler, clock_generator, noise_frame, noise_standard_deviation, orbit); return orbit_observer; } diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index 03963afe9..df22c807d 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -14,10 +14,10 @@ #include "../base/component.hpp" /** - * @enum ErrorFrame - * @brief Error definition frame + * @enum NoiseFrame + * @brief Noide definition frame */ -enum class ErrorFrame { +enum class NoiseFrame { kInertial, //!< Inertial frame kRtn, //!< RTN frame }; @@ -33,11 +33,11 @@ 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] noise_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 ErrorFrame error_frame, const libra::Vector<6> error_standard_deviation, + OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame, const libra::Vector<6> error_standard_deviation, const Orbit& orbit); /** @@ -81,13 +81,21 @@ class OrbitObserver : public Component, public ILoggable { 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] - ErrorFrame error_frame_; //!< Error definition frame + NoiseFrame noise_frame_; //!< Noise definition frame libra::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s] // Observed variables const Orbit& orbit_; //!< Orbit information }; +/** + * @fn SetNoiseFrame + * @brief Set NoiseFrame by string + * @param [in] noise_frame: Noise frame name + * @return noise frame + */ +NoiseFrame SetNoiseFrame(const std::string noise_frame); + /** * @fn InitStarSensor * @brief Initialize functions for StarSensor without power port From 3c6a1fec8820df7f2d7b92ca7b684da7780fcd05 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 7 Feb 2024 16:20:23 +0100 Subject: [PATCH 13/17] Add velocity observation --- src/components/ideal/orbit_observer.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 26d816933..07416e891 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -22,19 +22,26 @@ void OrbitObserver::MainRoutine(const int time_count) { // Calc noise libra::Vector<3> position_error_i_m{0.0}; libra::Vector<3> position_error_rtn_m{0.0}; + libra::Vector<3> velocity_error_i_m_s{0.0}; + libra::Vector<3> velocity_error_rtn_m_s{0.0}; libra::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh(); switch (noise_frame_) { case NoiseFrame::kInertial: for (size_t axis = 0; axis < 3; axis++) { position_error_i_m[axis] = normal_random_noise_[axis]; + velocity_error_i_m_s[axis] = normal_random_noise_[axis + 3]; } break; case NoiseFrame::kRtn: for (size_t axis = 0; axis < 3; axis++) { position_error_rtn_m[axis] = normal_random_noise_[axis]; + velocity_error_rtn_m_s[axis] = normal_random_noise_[axis + 3]; } // Frame conversion position_error_i_m = q_i2rtn.InverseFrameConversion(position_error_rtn_m); + // For zero bias noise, we do not need to care frame rotation effect. + velocity_error_i_m_s = q_i2rtn.InverseFrameConversion(velocity_error_rtn_m_s); + break; default: break; @@ -42,6 +49,7 @@ void OrbitObserver::MainRoutine(const int time_count) { // Get observed value observed_position_i_m_ = orbit_.GetPosition_i_m() + position_error_i_m; + observed_velocity_i_m_s_ = orbit_.GetVelocity_i_m_s() + velocity_error_i_m_s; } std::string OrbitObserver::GetLogHeader() const { From ae87d688acdaebce336648f6c1acd906aebfbbc2 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 9 Feb 2024 10:31:22 +0100 Subject: [PATCH 14/17] Fix typo --- src/components/ideal/orbit_observer.cpp | 4 ++-- src/components/ideal/orbit_observer.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp index 07416e891..61fe2b002 100644 --- a/src/components/ideal/orbit_observer.cpp +++ b/src/components/ideal/orbit_observer.cpp @@ -37,9 +37,9 @@ void OrbitObserver::MainRoutine(const int time_count) { position_error_rtn_m[axis] = normal_random_noise_[axis]; velocity_error_rtn_m_s[axis] = normal_random_noise_[axis + 3]; } - // Frame conversion + // Frame conversion position_error_i_m = q_i2rtn.InverseFrameConversion(position_error_rtn_m); - // For zero bias noise, we do not need to care frame rotation effect. + // For zero bias noise, we do not need to care the frame rotation effect. velocity_error_i_m_s = q_i2rtn.InverseFrameConversion(velocity_error_rtn_m_s); break; diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp index df22c807d..490489957 100644 --- a/src/components/ideal/orbit_observer.hpp +++ b/src/components/ideal/orbit_observer.hpp @@ -15,7 +15,7 @@ /** * @enum NoiseFrame - * @brief Noide definition frame + * @brief Noise definition frame */ enum class NoiseFrame { kInertial, //!< Inertial frame From 9180f53eb11f5694cfac21ccceb8a825dea8ed45 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 9 Feb 2024 10:53:26 +0100 Subject: [PATCH 15/17] Add instance to sample codes --- .../initialize_files/components/orbit_observer.ini | 12 ++++++------ data/sample/initialize_files/sample_satellite.ini | 1 + .../spacecraft/sample_components.cpp | 6 ++++++ .../spacecraft/sample_components.hpp | 2 ++ 4 files changed, 15 insertions(+), 6 deletions(-) diff --git a/data/sample/initialize_files/components/orbit_observer.ini b/data/sample/initialize_files/components/orbit_observer.ini index 8d527e618..1c27ecf57 100644 --- a/data/sample/initialize_files/components/orbit_observer.ini +++ b/data/sample/initialize_files/components/orbit_observer.ini @@ -6,12 +6,12 @@ noise_frame = INERTIAL // Standard deviation of position and velocity noise [m, m/s] // The frame definition can be selected above -noise_standard_deviation[0] = 1000 // Position-X -noise_standard_deviation[1] = 2000 // Position-Y -noise_standard_deviation[2] = 3000 // Position-Z -noise_standard_deviation[3] = 30 // Velocity-X -noise_standard_deviation[4] = 20 // Velocity-Y -noise_standard_deviation[5] = 10 // Velocity-Z +noise_standard_deviation(0) = 1000 // Position-X +noise_standard_deviation(1) = 2000 // Position-Y +noise_standard_deviation(2) = 3000 // Position-Z +noise_standard_deviation(3) = 30 // Velocity-X +noise_standard_deviation(4) = 20 // Velocity-Y +noise_standard_deviation(5) = 10 // Velocity-Z [COMPONENT_BASE] diff --git a/data/sample/initialize_files/sample_satellite.ini b/data/sample/initialize_files/sample_satellite.ini index 69dcd5210..d0fa9c23a 100644 --- a/data/sample/initialize_files/sample_satellite.ini +++ b/data/sample/initialize_files/sample_satellite.ini @@ -146,6 +146,7 @@ force_generator_file = INI_FILE_DIR_FROM_EXE/components/force_generator.ini torque_generator_file = INI_FILE_DIR_FROM_EXE/components/torque_generator.ini angular_velocity_observer_file = INI_FILE_DIR_FROM_EXE/components/angular_velocity_observer.ini attitude_observer_file = INI_FILE_DIR_FROM_EXE/components/attitude_observer.ini +orbit_observer_file = INI_FILE_DIR_FROM_EXE/components/orbit_observer.ini antenna_file = INI_FILE_DIR_FROM_EXE/components/spacecraft_antenna.ini component_interference_file = INI_FILE_DIR_FROM_EXE/components/component_interference.ini telescope_file = INI_FILE_DIR_FROM_EXE/components/telescope.ini diff --git a/src/simulation_sample/spacecraft/sample_components.cpp b/src/simulation_sample/spacecraft/sample_components.cpp index d529d040a..1d997625f 100644 --- a/src/simulation_sample/spacecraft/sample_components.cpp +++ b/src/simulation_sample/spacecraft/sample_components.cpp @@ -106,6 +106,11 @@ SampleComponents::SampleComponents(const Dynamics* dynamics, Structure* structur configuration_->main_logger_->CopyFileToLogDirectory(file_name); attitude_observer_ = new AttitudeObserver(InitializeAttitudeObserver(clock_generator, file_name, dynamics_->GetAttitude())); + // Orbit Observer + file_name = iniAccess.ReadString("COMPONENT_FILES", "orbit_observer_file"); + configuration_->main_logger_->CopyFileToLogDirectory(file_name); + orbit_observer_ = new OrbitObserver(InitializeOrbitObserver(clock_generator, file_name, dynamics_->GetOrbit())); + // Antenna file_name = iniAccess.ReadString("COMPONENT_FILES", "antenna_file"); configuration_->main_logger_->CopyFileToLogDirectory(file_name); @@ -171,6 +176,7 @@ SampleComponents::~SampleComponents() { delete torque_generator_; delete angular_velocity_observer_; delete attitude_observer_; + delete orbit_observer_; delete antenna_; delete mtq_magnetometer_interference_; // delete change_structure_; diff --git a/src/simulation_sample/spacecraft/sample_components.hpp b/src/simulation_sample/spacecraft/sample_components.hpp index 2b7574f27..abc355bef 100644 --- a/src/simulation_sample/spacecraft/sample_components.hpp +++ b/src/simulation_sample/spacecraft/sample_components.hpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -114,6 +115,7 @@ class SampleComponents : public InstalledComponents { TorqueGenerator* torque_generator_; //!< Ideal Torque Generator AngularVelocityObserver* angular_velocity_observer_; //!< Ideal Angular velocity observer AttitudeObserver* attitude_observer_; //!< Ideal Attitude observer + OrbitObserver* orbit_observer_; //!< Ideal Orbit observer // Mission Telescope* telescope_; //!< Telescope From cc4e1cfe499b9f9bfa65a3fdf07a34cab8fcae8d Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 9 Feb 2024 11:00:24 +0100 Subject: [PATCH 16/17] Add plot script for orbit observer --- scripts/Plot/plot_orbit_observer.py | 126 ++++++++++++++++++++++++++++ 1 file changed, 126 insertions(+) create mode 100644 scripts/Plot/plot_orbit_observer.py diff --git a/scripts/Plot/plot_orbit_observer.py b/scripts/Plot/plot_orbit_observer.py new file mode 100644 index 000000000..b84c02830 --- /dev/null +++ b/scripts/Plot/plot_orbit_observer.py @@ -0,0 +1,126 @@ +# +# Plot orbit observer +# +# arg[1] : read_file_tag : time tag for default CSV output log file. ex. 220627_142946 +# + +# +# Import +# +# plots +import matplotlib.pyplot as plt +# numpy +import numpy as np +# local function +from common import find_latest_log_tag +from common import add_log_file_arguments +from common import read_3d_vector_from_csv +from common import read_scalar_from_csv +# arguments +import argparse + +# Arguments +aparser = argparse.ArgumentParser() +aparser = add_log_file_arguments(aparser) +aparser.add_argument('--no-gui', action='store_true') +args = aparser.parse_args() + +# +# Read Arguments +# +# log file path +path_to_logs = args.logs_dir + +read_file_tag = args.file_tag +if read_file_tag == None: + print("file tag does not found. use latest.") + read_file_tag = find_latest_log_tag(path_to_logs) + +print("log: " + read_file_tag) + +# +# CSV file name +# +read_file_name = path_to_logs + '/' + 'logs_' + read_file_tag + '/' + read_file_tag + '_default.csv' + +# +# Data read and edit +# +# Read S2E CSV +time = read_scalar_from_csv(read_file_name, 'elapsed_time[s]') + +measured_position_eci_m = read_3d_vector_from_csv(read_file_name, 'orbit_observer_position_i', 'm') +true_position_eci_m = read_3d_vector_from_csv(read_file_name, 'spacecraft_position_i', 'm') + +measured_velocity_eci_m_s = read_3d_vector_from_csv(read_file_name, 'orbit_observer_velocity_i', 'm/s') +true_velocity_eci_m_s = read_3d_vector_from_csv(read_file_name, 'spacecraft_velocity_i', 'm/s') + +# Statistics +position_error_m = measured_position_eci_m[:, 1:] - true_position_eci_m[:, 1:] +position_average = [0.0, 0.0, 0.0] +position_standard_deviation = [0.0, 0.0, 0.0] +velocity_error_m = measured_velocity_eci_m_s[:, 1:] - true_velocity_eci_m_s[:, 1:] +velocity_average = [0.0, 0.0, 0.0] +velocity_standard_deviation = [0.0, 0.0, 0.0] +for i in range(3): + position_average[i] = position_error_m[i].mean() + position_standard_deviation[i] = position_error_m[i].std() + velocity_average[i] = velocity_error_m[i].mean() + velocity_standard_deviation[i] = velocity_error_m[i].std() + +# +# Plot +# +unit = ' m' +fig, axis = plt.subplots(3, 1, squeeze = False, tight_layout = True, sharex = True) +axis[0, 0].plot(time[0], measured_position_eci_m[0], marker=".", c="red", label="MEASURED-X") +axis[0, 0].plot(time[0], true_position_eci_m[0], marker=".", c="orange", label="TRUE-X") +axis[0, 0].text(0.01, 0.99, "Error average:" + format(position_average[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) +axis[0, 0].text(0.01, 0.79, "Standard deviation:" + format(position_standard_deviation[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) +axis[0, 0].legend(loc = 'upper right') + +axis[1, 0].plot(time[0], measured_position_eci_m[1], marker=".", c="green", label="MEASURED-Y") +axis[1, 0].plot(time[0], true_position_eci_m[1], marker=".", c="yellow", label="TRUE-Y") +axis[1, 0].text(0.01, 0.99, "Error average:" + format(position_average[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) +axis[1, 0].text(0.01, 0.79, "Standard deviation:" + format(position_standard_deviation[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) +axis[1, 0].legend(loc = 'upper right') + +axis[2, 0].plot(time[0], measured_position_eci_m[2], marker=".", c="blue", label="MEASURED-Z") +axis[2, 0].plot(time[0], true_position_eci_m[2], marker=".", c="purple", label="TRUE-Z") +axis[2, 0].text(0.01, 0.99, "Error average:" + format(position_average[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) +axis[2, 0].text(0.01, 0.79, "Standard deviation:" + format(position_standard_deviation[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) +axis[2, 0].legend(loc = 'upper right') + +fig.suptitle("Orbit observer position results @ ECI") +fig.supylabel("Position [m]") +fig.supxlabel("Time [s]") + +unit = ' m/s' +fig, axis = plt.subplots(3, 1, squeeze = False, tight_layout = True, sharex = True) +axis[0, 0].plot(time[0], measured_velocity_eci_m_s[0], marker=".", c="red", label="MEASURED-X") +axis[0, 0].plot(time[0], true_velocity_eci_m_s[0], marker=".", c="orange", label="TRUE-X") +axis[0, 0].text(0.01, 0.99, "Error average:" + format(velocity_average[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) +axis[0, 0].text(0.01, 0.79, "Standard deviation:" + format(velocity_standard_deviation[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) +axis[0, 0].legend(loc = 'upper right') + +axis[1, 0].plot(time[0], measured_velocity_eci_m_s[1], marker=".", c="green", label="MEASURED-Y") +axis[1, 0].plot(time[0], true_velocity_eci_m_s[1], marker=".", c="yellow", label="TRUE-Y") +axis[1, 0].text(0.01, 0.99, "Error average:" + format(velocity_average[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) +axis[1, 0].text(0.01, 0.79, "Standard deviation:" + format(velocity_standard_deviation[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) +axis[1, 0].legend(loc = 'upper right') + +axis[2, 0].plot(time[0], measured_velocity_eci_m_s[2], marker=".", c="blue", label="MEASURED-Z") +axis[2, 0].plot(time[0], true_velocity_eci_m_s[2], marker=".", c="purple", label="TRUE-Z") +axis[2, 0].text(0.01, 0.99, "Error average:" + format(velocity_average[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) +axis[2, 0].text(0.01, 0.79, "Standard deviation:" + format(velocity_standard_deviation[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) +axis[2, 0].legend(loc = 'upper right') + +fig.suptitle("Orbit observer velocity results @ ECI") +fig.supylabel("Velocity [m/s]") +fig.supxlabel("Time [s]") + +# Data save +if args.no_gui: + plt.savefig(read_file_tag + "_orbit_observer.png") # save last figure only +else: + plt.show() From e9dcc23fc192d9a794043216eb435d117b2c4912 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Fri, 9 Feb 2024 11:01:08 +0100 Subject: [PATCH 17/17] Add log output for orbit observer --- src/simulation_sample/spacecraft/sample_components.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/simulation_sample/spacecraft/sample_components.cpp b/src/simulation_sample/spacecraft/sample_components.cpp index 1d997625f..ffe4023a5 100644 --- a/src/simulation_sample/spacecraft/sample_components.cpp +++ b/src/simulation_sample/spacecraft/sample_components.cpp @@ -221,4 +221,5 @@ void SampleComponents::LogSetup(Logger& logger) { logger.AddLogList(torque_generator_); logger.AddLogList(angular_velocity_observer_); logger.AddLogList(attitude_observer_); + logger.AddLogList(orbit_observer_); }