generated from ut-issl/repository-template
-
Notifications
You must be signed in to change notification settings - Fork 18
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Add orbit observer #595
Merged
Merged
Add orbit observer #595
Changes from all commits
Commits
Show all changes
17 commits
Select commit
Hold shift + click to select a range
3104939
Add orbit observer
200km d174ef7
Add cpp file
200km 3fc4c0a
Implement main routine without noise
200km 4b38b43
dd to CMake
200km 6975859
Add velocity
200km fa55700
Add velocity
200km b8019e5
Add noise
200km 5f00035
Fix format
200km 475d4c4
Fix for build success
200km d9e76b0
Fix noise calculation
200km 000c5c1
Fix small
200km fa9d18d
Add initialize file
200km 3c6a1fe
Add velocity observation
200km ae87d68
Fix typo
200km 9180f53
Add instance to sample codes
200km cc4e1cf
Add plot script for orbit observer
200km e9dcc23
Add log output for orbit observer
200km File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
19 changes: 19 additions & 0 deletions
19
data/sample/initialize_files/components/orbit_observer.ini
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,102 @@ | ||
/* | ||
* @file orbit_observer.cpp | ||
* @brief Ideal component which can observe orbit | ||
*/ | ||
|
||
#include "orbit_observer.hpp" | ||
|
||
#include <library/initialize/initialize_file_access.hpp> | ||
#include <library/randomization/global_randomization.hpp> | ||
|
||
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), 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()); | ||
} | ||
} | ||
|
||
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::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 the frame rotation effect. | ||
velocity_error_i_m_s = q_i2rtn.InverseFrameConversion(velocity_error_rtn_m_s); | ||
|
||
break; | ||
default: | ||
break; | ||
} | ||
|
||
// 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 { | ||
std::string str_tmp = ""; | ||
|
||
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; | ||
} | ||
|
||
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; | ||
} | ||
|
||
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); | ||
|
||
// CompoBase | ||
int prescaler = ini_file.ReadInt("COMPONENT_BASE", "prescaler"); | ||
if (prescaler <= 1) prescaler = 1; | ||
|
||
// Noise | ||
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, noise_frame, noise_standard_deviation, orbit); | ||
|
||
return orbit_observer; | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,108 @@ | ||
/* | ||
* @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 <dynamics/orbit/orbit.hpp> | ||
#include <library/logger/loggable.hpp> | ||
#include <library/math/vector.hpp> | ||
#include <library/randomization/normal_randomization.hpp> | ||
|
||
#include "../base/component.hpp" | ||
|
||
/** | ||
* @enum NoiseFrame | ||
* @brief Noise definition frame | ||
*/ | ||
enum class NoiseFrame { | ||
kInertial, //!< Inertial frame | ||
kRtn, //!< RTN frame | ||
}; | ||
|
||
/* | ||
* @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] 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 NoiseFrame noise_frame, const libra::Vector<6> error_standard_deviation, | ||
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 position | ||
*/ | ||
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] | ||
libra::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s] | ||
|
||
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 | ||
* @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_ |
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
設定値をオーバーライドするのであれば warning を出した方がよいのではないでしょうか
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
このコンポ以外も全て同様になっているので、別issueで切り出して一気に対応したいと思います。
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
https://github.com/ut-issl/s2e-core/issues/624
でissueを作りました。