Skip to content

Commit c211efc

Browse files
200km200km
authored
Merge pull request #595 from ut-issl/feature/add-position-observer
Add orbit observer
2 parents ff55356 + e9dcc23 commit c211efc

File tree

8 files changed

+366
-0
lines changed

8 files changed

+366
-0
lines changed

‎data/sample/initialize_files/components/orbit_observer.ini

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
[ORBIT_OBSERVER]
2+
// Noise definition frame
3+
// INERTIAL: Inertial frame
4+
// RTN: RTN frame
5+
noise_frame = INERTIAL
6+
7+
// Standard deviation of position and velocity noise [m, m/s]
8+
// The frame definition can be selected above
9+
noise_standard_deviation(0) = 1000 // Position-X
10+
noise_standard_deviation(1) = 2000 // Position-Y
11+
noise_standard_deviation(2) = 3000 // Position-Z
12+
noise_standard_deviation(3) = 30 // Velocity-X
13+
noise_standard_deviation(4) = 20 // Velocity-Y
14+
noise_standard_deviation(5) = 10 // Velocity-Z
15+
16+
17+
[COMPONENT_BASE]
18+
// Prescaler with respect to the component update period
19+
prescaler = 1

‎data/sample/initialize_files/sample_satellite.ini

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -146,6 +146,7 @@ force_generator_file = INI_FILE_DIR_FROM_EXE/components/force_generator.ini
146146
torque_generator_file = INI_FILE_DIR_FROM_EXE/components/torque_generator.ini
147147
angular_velocity_observer_file = INI_FILE_DIR_FROM_EXE/components/angular_velocity_observer.ini
148148
attitude_observer_file = INI_FILE_DIR_FROM_EXE/components/attitude_observer.ini
149+
orbit_observer_file = INI_FILE_DIR_FROM_EXE/components/orbit_observer.ini
149150
antenna_file = INI_FILE_DIR_FROM_EXE/components/spacecraft_antenna.ini
150151
component_interference_file = INI_FILE_DIR_FROM_EXE/components/component_interference.ini
151152
telescope_file = INI_FILE_DIR_FROM_EXE/components/telescope.ini

‎scripts/Plot/plot_orbit_observer.py

Lines changed: 126 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,126 @@
1+
#
2+
# Plot orbit observer
3+
#
4+
# arg[1] : read_file_tag : time tag for default CSV output log file. ex. 220627_142946
5+
#
6+
7+
#
8+
# Import
9+
#
10+
# plots
11+
import matplotlib.pyplot as plt
12+
# numpy
13+
import numpy as np
14+
# local function
15+
from common import find_latest_log_tag
16+
from common import add_log_file_arguments
17+
from common import read_3d_vector_from_csv
18+
from common import read_scalar_from_csv
19+
# arguments
20+
import argparse
21+
22+
# Arguments
23+
aparser = argparse.ArgumentParser()
24+
aparser = add_log_file_arguments(aparser)
25+
aparser.add_argument('--no-gui', action='store_true')
26+
args = aparser.parse_args()
27+
28+
#
29+
# Read Arguments
30+
#
31+
# log file path
32+
path_to_logs = args.logs_dir
33+
34+
read_file_tag = args.file_tag
35+
if read_file_tag == None:
36+
print("file tag does not found. use latest.")
37+
read_file_tag = find_latest_log_tag(path_to_logs)
38+
39+
print("log: " + read_file_tag)
40+
41+
#
42+
# CSV file name
43+
#
44+
read_file_name = path_to_logs + '/' + 'logs_' + read_file_tag + '/' + read_file_tag + '_default.csv'
45+
46+
#
47+
# Data read and edit
48+
#
49+
# Read S2E CSV
50+
time = read_scalar_from_csv(read_file_name, 'elapsed_time[s]')
51+
52+
measured_position_eci_m = read_3d_vector_from_csv(read_file_name, 'orbit_observer_position_i', 'm')
53+
true_position_eci_m = read_3d_vector_from_csv(read_file_name, 'spacecraft_position_i', 'm')
54+
55+
measured_velocity_eci_m_s = read_3d_vector_from_csv(read_file_name, 'orbit_observer_velocity_i', 'm/s')
56+
true_velocity_eci_m_s = read_3d_vector_from_csv(read_file_name, 'spacecraft_velocity_i', 'm/s')
57+
58+
# Statistics
59+
position_error_m = measured_position_eci_m[:, 1:] - true_position_eci_m[:, 1:]
60+
position_average = [0.0, 0.0, 0.0]
61+
position_standard_deviation = [0.0, 0.0, 0.0]
62+
velocity_error_m = measured_velocity_eci_m_s[:, 1:] - true_velocity_eci_m_s[:, 1:]
63+
velocity_average = [0.0, 0.0, 0.0]
64+
velocity_standard_deviation = [0.0, 0.0, 0.0]
65+
for i in range(3):
66+
position_average[i] = position_error_m[i].mean()
67+
position_standard_deviation[i] = position_error_m[i].std()
68+
velocity_average[i] = velocity_error_m[i].mean()
69+
velocity_standard_deviation[i] = velocity_error_m[i].std()
70+
71+
#
72+
# Plot
73+
#
74+
unit = ' m'
75+
fig, axis = plt.subplots(3, 1, squeeze = False, tight_layout = True, sharex = True)
76+
axis[0, 0].plot(time[0], measured_position_eci_m[0], marker=".", c="red", label="MEASURED-X")
77+
axis[0, 0].plot(time[0], true_position_eci_m[0], marker=".", c="orange", label="TRUE-X")
78+
axis[0, 0].text(0.01, 0.99, "Error average:" + format(position_average[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes)
79+
axis[0, 0].text(0.01, 0.79, "Standard deviation:" + format(position_standard_deviation[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes)
80+
axis[0, 0].legend(loc = 'upper right')
81+
82+
axis[1, 0].plot(time[0], measured_position_eci_m[1], marker=".", c="green", label="MEASURED-Y")
83+
axis[1, 0].plot(time[0], true_position_eci_m[1], marker=".", c="yellow", label="TRUE-Y")
84+
axis[1, 0].text(0.01, 0.99, "Error average:" + format(position_average[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes)
85+
axis[1, 0].text(0.01, 0.79, "Standard deviation:" + format(position_standard_deviation[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes)
86+
axis[1, 0].legend(loc = 'upper right')
87+
88+
axis[2, 0].plot(time[0], measured_position_eci_m[2], marker=".", c="blue", label="MEASURED-Z")
89+
axis[2, 0].plot(time[0], true_position_eci_m[2], marker=".", c="purple", label="TRUE-Z")
90+
axis[2, 0].text(0.01, 0.99, "Error average:" + format(position_average[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes)
91+
axis[2, 0].text(0.01, 0.79, "Standard deviation:" + format(position_standard_deviation[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes)
92+
axis[2, 0].legend(loc = 'upper right')
93+
94+
fig.suptitle("Orbit observer position results @ ECI")
95+
fig.supylabel("Position [m]")
96+
fig.supxlabel("Time [s]")
97+
98+
unit = ' m/s'
99+
fig, axis = plt.subplots(3, 1, squeeze = False, tight_layout = True, sharex = True)
100+
axis[0, 0].plot(time[0], measured_velocity_eci_m_s[0], marker=".", c="red", label="MEASURED-X")
101+
axis[0, 0].plot(time[0], true_velocity_eci_m_s[0], marker=".", c="orange", label="TRUE-X")
102+
axis[0, 0].text(0.01, 0.99, "Error average:" + format(velocity_average[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes)
103+
axis[0, 0].text(0.01, 0.79, "Standard deviation:" + format(velocity_standard_deviation[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes)
104+
axis[0, 0].legend(loc = 'upper right')
105+
106+
axis[1, 0].plot(time[0], measured_velocity_eci_m_s[1], marker=".", c="green", label="MEASURED-Y")
107+
axis[1, 0].plot(time[0], true_velocity_eci_m_s[1], marker=".", c="yellow", label="TRUE-Y")
108+
axis[1, 0].text(0.01, 0.99, "Error average:" + format(velocity_average[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes)
109+
axis[1, 0].text(0.01, 0.79, "Standard deviation:" + format(velocity_standard_deviation[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes)
110+
axis[1, 0].legend(loc = 'upper right')
111+
112+
axis[2, 0].plot(time[0], measured_velocity_eci_m_s[2], marker=".", c="blue", label="MEASURED-Z")
113+
axis[2, 0].plot(time[0], true_velocity_eci_m_s[2], marker=".", c="purple", label="TRUE-Z")
114+
axis[2, 0].text(0.01, 0.99, "Error average:" + format(velocity_average[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes)
115+
axis[2, 0].text(0.01, 0.79, "Standard deviation:" + format(velocity_standard_deviation[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes)
116+
axis[2, 0].legend(loc = 'upper right')
117+
118+
fig.suptitle("Orbit observer velocity results @ ECI")
119+
fig.supylabel("Velocity [m/s]")
120+
fig.supxlabel("Time [s]")
121+
122+
# Data save
123+
if args.no_gui:
124+
plt.savefig(read_file_tag + "_orbit_observer.png") # save last figure only
125+
else:
126+
plt.show()

‎src/components/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@ ideal/force_generator.cpp
3737
ideal/torque_generator.cpp
3838
ideal/angular_velocity_observer.cpp
3939
ideal/attitude_observer.cpp
40+
ideal/orbit_observer.cpp
4041

4142
real/mission/telescope.cpp
4243

‎src/components/ideal/orbit_observer.cpp

Lines changed: 102 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,102 @@
1+
/*
2+
* @file orbit_observer.cpp
3+
* @brief Ideal component which can observe orbit
4+
*/
5+
6+
#include "orbit_observer.hpp"
7+
8+
#include <library/initialize/initialize_file_access.hpp>
9+
#include <library/randomization/global_randomization.hpp>
10+
11+
OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame,
12+
const libra::Vector<6> error_standard_deviation, const Orbit& orbit)
13+
: Component(prescaler, clock_generator), noise_frame_(noise_frame), orbit_(orbit) {
14+
for (size_t i = 0; i < 6; i++) {
15+
normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], global_randomization.MakeSeed());
16+
}
17+
}
18+
19+
void OrbitObserver::MainRoutine(const int time_count) {
20+
UNUSED(time_count);
21+
22+
// Calc noise
23+
libra::Vector<3> position_error_i_m{0.0};
24+
libra::Vector<3> position_error_rtn_m{0.0};
25+
libra::Vector<3> velocity_error_i_m_s{0.0};
26+
libra::Vector<3> velocity_error_rtn_m_s{0.0};
27+
libra::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh();
28+
switch (noise_frame_) {
29+
case NoiseFrame::kInertial:
30+
for (size_t axis = 0; axis < 3; axis++) {
31+
position_error_i_m[axis] = normal_random_noise_[axis];
32+
velocity_error_i_m_s[axis] = normal_random_noise_[axis + 3];
33+
}
34+
break;
35+
case NoiseFrame::kRtn:
36+
for (size_t axis = 0; axis < 3; axis++) {
37+
position_error_rtn_m[axis] = normal_random_noise_[axis];
38+
velocity_error_rtn_m_s[axis] = normal_random_noise_[axis + 3];
39+
}
40+
// Frame conversion
41+
position_error_i_m = q_i2rtn.InverseFrameConversion(position_error_rtn_m);
42+
// For zero bias noise, we do not need to care the frame rotation effect.
43+
velocity_error_i_m_s = q_i2rtn.InverseFrameConversion(velocity_error_rtn_m_s);
44+
45+
break;
46+
default:
47+
break;
48+
}
49+
50+
// Get observed value
51+
observed_position_i_m_ = orbit_.GetPosition_i_m() + position_error_i_m;
52+
observed_velocity_i_m_s_ = orbit_.GetVelocity_i_m_s() + velocity_error_i_m_s;
53+
}
54+
55+
std::string OrbitObserver::GetLogHeader() const {
56+
std::string str_tmp = "";
57+
58+
std::string head = "orbit_observer_";
59+
str_tmp += WriteVector(head + "position", "i", "m", 3);
60+
str_tmp += WriteVector(head + "velocity", "i", "m/s", 3);
61+
62+
return str_tmp;
63+
}
64+
65+
std::string OrbitObserver::GetLogValue() const {
66+
std::string str_tmp = "";
67+
68+
str_tmp += WriteVector(observed_position_i_m_, 16);
69+
str_tmp += WriteVector(observed_velocity_i_m_s_, 16);
70+
71+
return str_tmp;
72+
}
73+
74+
NoiseFrame SetNoiseFrame(const std::string noise_frame) {
75+
if (noise_frame == "INERTIAL") {
76+
return NoiseFrame::kInertial;
77+
} else if (noise_frame == "RTN") {
78+
return NoiseFrame::kRtn;
79+
} else {
80+
std::cerr << "[WARNINGS] Orbit observer noise frame is not defined!" << std::endl;
81+
std::cerr << "The noise frame is automatically initialized as INERTIAL" << std::endl;
82+
return NoiseFrame::kInertial;
83+
}
84+
}
85+
86+
OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) {
87+
// General
88+
IniAccess ini_file(file_name);
89+
90+
// CompoBase
91+
int prescaler = ini_file.ReadInt("COMPONENT_BASE", "prescaler");
92+
if (prescaler <= 1) prescaler = 1;
93+
94+
// Noise
95+
const NoiseFrame noise_frame = SetNoiseFrame(ini_file.ReadString("ORBIT_OBSERVER", "noise_frame"));
96+
libra::Vector<6> noise_standard_deviation;
97+
ini_file.ReadVector("ORBIT_OBSERVER", "noise_standard_deviation", noise_standard_deviation);
98+
99+
OrbitObserver orbit_observer(prescaler, clock_generator, noise_frame, noise_standard_deviation, orbit);
100+
101+
return orbit_observer;
102+
}

‎src/components/ideal/orbit_observer.hpp

Lines changed: 108 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,108 @@
1+
/*
2+
* @file orbit_observer.hpp
3+
* @brief Ideal component which can observe orbit
4+
*/
5+
6+
#ifndef S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_
7+
#define S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_
8+
9+
#include <dynamics/orbit/orbit.hpp>
10+
#include <library/logger/loggable.hpp>
11+
#include <library/math/vector.hpp>
12+
#include <library/randomization/normal_randomization.hpp>
13+
14+
#include "../base/component.hpp"
15+
16+
/**
17+
* @enum NoiseFrame
18+
* @brief Noise definition frame
19+
*/
20+
enum class NoiseFrame {
21+
kInertial, //!< Inertial frame
22+
kRtn, //!< RTN frame
23+
};
24+
25+
/*
26+
* @class OrbitObserver
27+
* @brief Ideal component which can observe orbit
28+
*/
29+
class OrbitObserver : public Component, public ILoggable {
30+
public:
31+
/**
32+
* @fn OrbitObserver
33+
* @brief Constructor without power port
34+
* @param [in] prescaler: Frequency scale factor for update
35+
* @param [in] clock_generator: Clock generator
36+
* @param [in] noise_frame: Error frame definition
37+
* @param [in] error_standard_deviation: Position and Velocity standard deviation noise [m, m/s]
38+
* @param [in] orbit: Orbit information
39+
*/
40+
OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame, const libra::Vector<6> error_standard_deviation,
41+
const Orbit& orbit);
42+
43+
/**
44+
* @fn ~AttitudeObserver
45+
* @brief Destructor
46+
*/
47+
~OrbitObserver() {}
48+
49+
// Override functions for Component
50+
/**
51+
* @fn MainRoutine
52+
* @brief Main routine for sensor observation
53+
*/
54+
void MainRoutine(const int time_count) override;
55+
56+
// Override ILoggable
57+
/**
58+
* @fn GetLogHeader
59+
* @brief Override GetLogHeader function of ILoggable
60+
*/
61+
virtual std::string GetLogHeader() const override;
62+
/**
63+
* @fn GetLogValue
64+
* @brief Override GetLogValue function of ILoggable
65+
*/
66+
virtual std::string GetLogValue() const override;
67+
68+
/**
69+
* @fn GetPosition_i_m
70+
* @brief Return observed position
71+
*/
72+
inline const libra::Vector<3> GetPosition_i_m() const { return observed_position_i_m_; };
73+
74+
/**
75+
* @fn GetVelocity_i_m_s
76+
* @brief Return observed velocity
77+
*/
78+
inline const libra::Vector<3> GetVelocity_i_m_s() const { return observed_velocity_i_m_s_; };
79+
80+
protected:
81+
libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m]
82+
libra::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s]
83+
84+
NoiseFrame noise_frame_; //!< Noise definition frame
85+
libra::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s]
86+
87+
// Observed variables
88+
const Orbit& orbit_; //!< Orbit information
89+
};
90+
91+
/**
92+
* @fn SetNoiseFrame
93+
* @brief Set NoiseFrame by string
94+
* @param [in] noise_frame: Noise frame name
95+
* @return noise frame
96+
*/
97+
NoiseFrame SetNoiseFrame(const std::string noise_frame);
98+
99+
/**
100+
* @fn InitStarSensor
101+
* @brief Initialize functions for StarSensor without power port
102+
* @param [in] clock_generator: Clock generator
103+
* @param [in] file_name: Path to the initialize file
104+
* @param [in] orbit: Orbit information
105+
*/
106+
OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit);
107+
108+
#endif // S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_

0 commit comments

Comments
 (0)