Skip to content

Commit

Permalink
Merge pull request #656 from ut-issl/feature/fix-name-space
Browse files Browse the repository at this point in the history 
Fix namespace: libra
  • Loading branch information
@200km
200km authored Aug 13, 2024
2 parents 3f6930a + 1a40df5 commit 768107b
Show file tree
Hide file tree
Showing 224 changed files with 1,995 additions and 1,852 deletions.
22 changes: 11 additions & 11 deletions src/components/base/sensor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,40 +31,40 @@ class Sensor {
* @param [in] random_walk_standard_deviation_c: Standard deviation of random wark at the component frame
* @param [in] random_walk_limit_c: Limit of random walk at the component frame
*/
Sensor(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_noise_c, const libra::Vector<N>& normal_random_standard_deviation_c, const double random_walk_step_width_s,
const libra::Vector<N>& random_walk_standard_deviation_c, const libra::Vector<N>& random_walk_limit_c);
Sensor(const math::Matrix<N, N>& scale_factor, const math::Vector<N>& range_to_const_c, const math::Vector<N>& range_to_zero_c,
const math::Vector<N>& bias_noise_c, const math::Vector<N>& normal_random_standard_deviation_c, const double random_walk_step_width_s,
const math::Vector<N>& random_walk_standard_deviation_c, const math::Vector<N>& random_walk_limit_c);
/**
* @fn ~Sensor
* @brief Destructor
*/
~Sensor();

protected:
libra::Vector<N> bias_noise_c_; //!< Constant bias noise at the component frame
math::Vector<N> bias_noise_c_; //!< Constant bias noise at the component frame

/**
* @fn Measure
* @brief Return the observed data after adding the noise
* @param [in] true_value_c: True value at the component frame
* @return Observed value with noise at the component frame
*/
libra::Vector<N> Measure(const libra::Vector<N> true_value_c);
math::Vector<N> Measure(const math::Vector<N> true_value_c);

private:
libra::Matrix<N, N> scale_factor_; //!< Scale factor matrix
libra::Vector<N> range_to_const_c_; //!< Output range limit to be constant output value at the component frame
libra::Vector<N> range_to_zero_c_; //!< Output range limit to be zero output value at the component frame
libra::NormalRand normal_random_noise_c_[N]; //!< Normal random
RandomWalk<N> random_walk_noise_c_; //!< Random Walk
math::Matrix<N, N> scale_factor_; //!< Scale factor matrix
math::Vector<N> range_to_const_c_; //!< Output range limit to be constant output value at the component frame
math::Vector<N> range_to_zero_c_; //!< Output range limit to be zero output value at the component frame
randomization::NormalRand normal_random_noise_c_[N]; //!< Normal random
RandomWalk<N> random_walk_noise_c_; //!< Random Walk

/**
* @fn Clip
* @brief Clipping according to the range information
* @param [in] input_c: Input value at the component frame
* @return Clipped value
*/
libra::Vector<N> Clip(const libra::Vector<N> input_c);
math::Vector<N> Clip(const math::Vector<N> input_c);
/**
* @fn RangeCheck
* @brief Check the range_to_const_c_ and range_to_zero_c_ is correct and fixed the values
Expand Down
34 changes: 17 additions & 17 deletions src/components/base/sensor_template_functions.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,10 @@
#include <setting_file_reader/initialize_file_access.hpp>

template <size_t N>
Sensor<N>::Sensor(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_noise_c, const libra::Vector<N>& normal_random_standard_deviation_c,
const double random_walk_step_width_s, const libra::Vector<N>& random_walk_standard_deviation_c,
const libra::Vector<N>& random_walk_limit_c)
Sensor<N>::Sensor(const math::Matrix<N, N>& scale_factor, const math::Vector<N>& range_to_const_c, const math::Vector<N>& range_to_zero_c,
const math::Vector<N>& bias_noise_c, const math::Vector<N>& normal_random_standard_deviation_c,
const double random_walk_step_width_s, const math::Vector<N>& random_walk_standard_deviation_c,
const math::Vector<N>& random_walk_limit_c)
: bias_noise_c_(bias_noise_c),
scale_factor_(scale_factor),
range_to_const_c_(range_to_const_c),
Expand All @@ -29,8 +29,8 @@ template <size_t N>
Sensor<N>::~Sensor() {}

template <size_t N>
libra::Vector<N> Sensor<N>::Measure(const libra::Vector<N> true_value_c) {
libra::Vector<N> calc_value_c;
math::Vector<N> Sensor<N>::Measure(const math::Vector<N> true_value_c) {
math::Vector<N> calc_value_c;
calc_value_c = scale_factor_ * true_value_c;
calc_value_c += bias_noise_c_;
for (size_t i = 0; i < N; ++i) {
Expand All @@ -42,8 +42,8 @@ libra::Vector<N> Sensor<N>::Measure(const libra::Vector<N> true_value_c) {
}

template <size_t N>
libra::Vector<N> Sensor<N>::Clip(const libra::Vector<N> input_c) {
libra::Vector<N> output_c;
math::Vector<N> Sensor<N>::Clip(const math::Vector<N> input_c) {
math::Vector<N> output_c;
for (size_t i = 0; i < N; ++i) {
if (input_c[i] >= range_to_const_c_[i] && input_c[i] < range_to_zero_c_[i]) {
output_c[i] = range_to_const_c_[i];
Expand Down Expand Up @@ -80,11 +80,11 @@ Sensor<N> ReadSensorInformation(const std::string file_name, const double step_w
IniAccess ini_file(file_name);
std::string section = "SENSOR_BASE_" + component_name;

libra::Vector<N * N> scale_factor_vector;
math::Vector<N * N> scale_factor_vector;
ini_file.ReadVector(section.c_str(), "scale_factor_c", scale_factor_vector);
libra::Matrix<N, N> scale_factor_c;
math::Matrix<N, N> scale_factor_c;
if (scale_factor_vector.CalcNorm() == 0.0) {
scale_factor_c = libra::MakeIdentityMatrix<N>();
scale_factor_c = math::MakeIdentityMatrix<N>();
} else {
for (size_t i = 0; i < N; i++) {
for (size_t j = 0; j < N; j++) {
Expand All @@ -94,26 +94,26 @@ Sensor<N> ReadSensorInformation(const std::string file_name, const double step_w
}

std::string key_name;
libra::Vector<N> constant_bias_c;
math::Vector<N> constant_bias_c;
key_name = "constant_bias_c_" + unit;
ini_file.ReadVector(section.c_str(), key_name.c_str(), constant_bias_c);
libra::Vector<N> normal_random_standard_deviation_c;
math::Vector<N> normal_random_standard_deviation_c;
key_name = "normal_random_standard_deviation_c_" + unit;
ini_file.ReadVector(section.c_str(), key_name.c_str(), normal_random_standard_deviation_c);
libra::Vector<N> random_walk_standard_deviation_c;
math::Vector<N> random_walk_standard_deviation_c;
key_name = "random_walk_standard_deviation_c_" + unit;
ini_file.ReadVector(section.c_str(), key_name.c_str(), random_walk_standard_deviation_c);
libra::Vector<N> random_walk_limit_c;
math::Vector<N> random_walk_limit_c;
key_name = "random_walk_limit_c_" + unit;
ini_file.ReadVector(section.c_str(), key_name.c_str(), random_walk_limit_c);

key_name = "range_to_constant_" + unit;
double range_to_const = ini_file.ReadDouble(section.c_str(), key_name.c_str());
libra::Vector<N> range_to_const_c{range_to_const};
math::Vector<N> range_to_const_c{range_to_const};

key_name = "range_to_zero_" + unit;
double range_to_zero = ini_file.ReadDouble(section.c_str(), key_name.c_str());
libra::Vector<N> range_to_zero_c{range_to_zero};
math::Vector<N> range_to_zero_c{range_to_zero};

Sensor<N> sensor_base(scale_factor_c, range_to_const_c, range_to_zero_c, constant_bias_c, normal_random_standard_deviation_c, step_width_s,
random_walk_standard_deviation_c, random_walk_limit_c);
Expand Down
2 changes: 1 addition & 1 deletion src/components/examples/example_change_structure.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ void ExampleChangeStructure::MainRoutine(const int time_count) {
structure_->GetToSetSurfaces()[0].SetArea_m2(0.5);

// Inertia Tensor
libra::Matrix<3, 3> inertia_tensor_b_kgm2(0.0);
math::Matrix<3, 3> inertia_tensor_b_kgm2(0.0);
inertia_tensor_b_kgm2[0][0] = 0.2;
inertia_tensor_b_kgm2[1][1] = 0.2;
inertia_tensor_b_kgm2[2][2] = 0.2;
Expand Down
6 changes: 3 additions & 3 deletions src/components/ideal/angular_velocity_observer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,11 +57,11 @@ class AngularVelocityObserver : public Component, public Sensor<3>, public ILogg
* @fn GetAngularVelocity_b_rad_s
* @brief Return observed angular velocity
*/
inline libra::Vector<3> GetAngularVelocity_b_rad_s() const { return angular_velocity_b_rad_s_; }
inline math::Vector<3> GetAngularVelocity_b_rad_s() const { return angular_velocity_b_rad_s_; }

protected:
libra::Vector<3> angular_velocity_b_rad_s_{0.0}; //!< Observed angular velocity [rad/s]
const Attitude& attitude_; //!< Dynamics information
math::Vector<3> angular_velocity_b_rad_s_{0.0}; //!< Observed angular velocity [rad/s]
const Attitude& attitude_; //!< Dynamics information
};

/**
Expand Down
6 changes: 3 additions & 3 deletions src/components/ideal/attitude_observer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,14 +18,14 @@ void AttitudeObserver::MainRoutine(const int time_count) {
UNUSED(time_count);

// Error calculation
libra::Vector<3> random_direction;
math::Vector<3> random_direction;
random_direction[0] = direction_noise_;
random_direction[1] = direction_noise_;
random_direction[2] = direction_noise_;
random_direction = random_direction.CalcNormalizedVector();

double error_angle_rad = angle_noise_;
libra::Quaternion error_quaternion(random_direction, error_angle_rad);
math::Quaternion error_quaternion(random_direction, error_angle_rad);

observed_quaternion_i2b_ = error_quaternion * attitude_.GetQuaternion_i2b();
}
Expand Down Expand Up @@ -57,7 +57,7 @@ AttitudeObserver InitializeAttitudeObserver(ClockGenerator* clock_generator, con

// AttitudeObserver
double error_angle_standard_deviation_deg = ini_file.ReadDouble("ATTITUDE_OBSERVER", "error_angle_standard_deviation_deg");
double error_angle_standard_deviation_rad = libra::deg_to_rad * error_angle_standard_deviation_deg;
double error_angle_standard_deviation_rad = math::deg_to_rad * error_angle_standard_deviation_deg;
AttitudeObserver attitude_observer(prescaler, clock_generator, error_angle_standard_deviation_rad, attitude);

return attitude_observer;
Expand Down
8 changes: 4 additions & 4 deletions src/components/ideal/attitude_observer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,13 +57,13 @@ class AttitudeObserver : public Component, public ILoggable {
* @fn GetQuaternion_i2c
* @brief Return observed quaternion from the inertial frame to the body-fixed frame
*/
inline const libra::Quaternion GetQuaternion_i2b() const { return observed_quaternion_i2b_; };
inline const math::Quaternion GetQuaternion_i2b() const { return observed_quaternion_i2b_; };

protected:
libra::Quaternion observed_quaternion_i2b_ = {0.0, 0.0, 0.0, 1.0}; //!< Observed quaternion
math::Quaternion observed_quaternion_i2b_ = {0.0, 0.0, 0.0, 1.0}; //!< Observed quaternion

libra::NormalRand angle_noise_; //!< Normal random for magnitude noise
libra::NormalRand direction_noise_; //!< Normal random for direction noise
randomization::NormalRand angle_noise_; //!< Normal random for magnitude noise
randomization::NormalRand direction_noise_; //!< Normal random for direction noise

const Attitude& attitude_; //!< Attitude information
};
Expand Down
32 changes: 16 additions & 16 deletions src/components/ideal/force_generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,16 +29,16 @@ void ForceGenerator::MainRoutine(const int time_count) {
double norm_ordered_force = ordered_force_b_N_.CalcNorm();
if (norm_ordered_force > 0.0 + DBL_EPSILON) {
// Add noise only when the force is generated
libra::Vector<3> true_direction = generated_force_b_N_.CalcNormalizedVector();
libra::Quaternion error_quaternion = GenerateDirectionNoiseQuaternion(true_direction, direction_error_standard_deviation_rad_);
libra::Vector<3> converted_direction = error_quaternion.FrameConversion(true_direction);
math::Vector<3> true_direction = generated_force_b_N_.CalcNormalizedVector();
math::Quaternion error_quaternion = GenerateDirectionNoiseQuaternion(true_direction, direction_error_standard_deviation_rad_);
math::Vector<3> converted_direction = error_quaternion.FrameConversion(true_direction);
double force_norm_with_error = norm_ordered_force + magnitude_noise_;
generated_force_b_N_ = force_norm_with_error * converted_direction;
}

// Convert frame
libra::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b();
libra::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternion_i2lvlh();
math::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b();
math::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternion_i2lvlh();
generated_force_i_N_ = q_i2b.InverseFrameConversion(generated_force_b_N_);
generated_force_rtn_N_ = q_i2rtn.FrameConversion(generated_force_i_N_);
}
Expand All @@ -49,16 +49,16 @@ void ForceGenerator::PowerOffRoutine() {
generated_force_rtn_N_ *= 0.0;
}

void ForceGenerator::SetForce_i_N(const libra::Vector<3> force_i_N) {
libra::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b();
void ForceGenerator::SetForce_i_N(const math::Vector<3> force_i_N) {
math::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b();
ordered_force_b_N_ = q_i2b.FrameConversion(force_i_N);
}

void ForceGenerator::SetForce_rtn_N(const libra::Vector<3> force_rtn_N) {
libra::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b();
libra::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternion_i2lvlh();
void ForceGenerator::SetForce_rtn_N(const math::Vector<3> force_rtn_N) {
math::Quaternion q_i2b = dynamics_->GetAttitude().GetQuaternion_i2b();
math::Quaternion q_i2rtn = dynamics_->GetOrbit().CalcQuaternion_i2lvlh();

libra::Vector<3> force_i_N = q_i2rtn.InverseFrameConversion(force_rtn_N);
math::Vector<3> force_i_N = q_i2rtn.InverseFrameConversion(force_rtn_N);
ordered_force_b_N_ = q_i2b.FrameConversion(force_i_N);
}

Expand All @@ -85,14 +85,14 @@ std::string ForceGenerator::GetLogValue() const {
return str_tmp;
}

libra::Quaternion ForceGenerator::GenerateDirectionNoiseQuaternion(libra::Vector<3> true_direction, const double error_standard_deviation_rad) {
libra::Vector<3> random_direction;
math::Quaternion ForceGenerator::GenerateDirectionNoiseQuaternion(math::Vector<3> true_direction, const double error_standard_deviation_rad) {
math::Vector<3> random_direction;
random_direction[0] = direction_noise_;
random_direction[1] = direction_noise_;
random_direction[2] = direction_noise_;
random_direction = random_direction.CalcNormalizedVector();

libra::Vector<3> rotation_axis;
math::Vector<3> rotation_axis;
rotation_axis = OuterProduct(true_direction, random_direction);
double norm_rotation_axis = rotation_axis.CalcNorm();
if (norm_rotation_axis < 0.0 + DBL_EPSILON) {
Expand All @@ -101,7 +101,7 @@ libra::Quaternion ForceGenerator::GenerateDirectionNoiseQuaternion(libra::Vector
}

double error_angle_rad = direction_noise_ * error_standard_deviation_rad;
libra::Quaternion error_quaternion(rotation_axis, error_angle_rad);
math::Quaternion error_quaternion(rotation_axis, error_angle_rad);
return error_quaternion;
}

Expand All @@ -117,7 +117,7 @@ ForceGenerator InitializeForceGenerator(ClockGenerator* clock_generator, const s
char section[30] = "FORCE_GENERATOR";
double force_magnitude_standard_deviation_N = ini_file.ReadDouble(section, "force_magnitude_standard_deviation_N");
double force_direction_standard_deviation_deg = ini_file.ReadDouble(section, "force_direction_standard_deviation_deg");
double force_direction_standard_deviation_rad = libra::deg_to_rad * force_direction_standard_deviation_deg;
double force_direction_standard_deviation_rad = math::deg_to_rad * force_direction_standard_deviation_deg;
ForceGenerator force_generator(prescaler, clock_generator, force_magnitude_standard_deviation_N, force_direction_standard_deviation_rad, dynamics);

return force_generator;
Expand Down
20 changes: 10 additions & 10 deletions src/components/ideal/force_generator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,27 +81,27 @@ class ForceGenerator : public Component, public ILoggable {
* @fn SetForce_b_N
* @brief Set ordered force in the body fixed frame [N]
*/
inline void SetForce_b_N(const libra::Vector<3> force_b_N) { ordered_force_b_N_ = force_b_N; };
inline void SetForce_b_N(const math::Vector<3> force_b_N) { ordered_force_b_N_ = force_b_N; };
/**
* @fn SetForce_i_N
* @brief Set ordered force in the inertial frame [N]
*/
void SetForce_i_N(const libra::Vector<3> force_i_N);
void SetForce_i_N(const math::Vector<3> force_i_N);
/**
* @fn SetForce_rtn_N
* @brief Set ordered force in the RTN frame [N]
*/
void SetForce_rtn_N(const libra::Vector<3> force_rtn_N);
void SetForce_rtn_N(const math::Vector<3> force_rtn_N);

protected:
libra::Vector<3> ordered_force_b_N_{0.0}; //!< Ordered force in the body fixed frame [N]
libra::Vector<3> generated_force_b_N_{0.0}; //!< Generated force in the body fixed frame [N]
libra::Vector<3> generated_force_i_N_{0.0}; //!< Generated force in the inertial frame [N]
libra::Vector<3> generated_force_rtn_N_{0.0}; //!< Generated force in the RTN frame [N]
math::Vector<3> ordered_force_b_N_{0.0}; //!< Ordered force in the body fixed frame [N]
math::Vector<3> generated_force_b_N_{0.0}; //!< Generated force in the body fixed frame [N]
math::Vector<3> generated_force_i_N_{0.0}; //!< Generated force in the inertial frame [N]
math::Vector<3> generated_force_rtn_N_{0.0}; //!< Generated force in the RTN frame [N]

// Noise
libra::NormalRand magnitude_noise_; //!< Normal random for magnitude noise
libra::NormalRand direction_noise_; //!< Normal random for direction noise
randomization::NormalRand magnitude_noise_; //!< Normal random for magnitude noise
randomization::NormalRand direction_noise_; //!< Normal random for direction noise
double direction_error_standard_deviation_rad_; //!< Standard deviation of direction error [rad]

/**
Expand All @@ -110,7 +110,7 @@ class ForceGenerator : public Component, public ILoggable {
* @param [in] true_direction: True direction
* @param [in] error_standard_deviation_rad: Standard deviation of direction error [rad]
*/
libra::Quaternion GenerateDirectionNoiseQuaternion(libra::Vector<3> true_direction, const double error_standard_deviation_rad);
math::Quaternion GenerateDirectionNoiseQuaternion(math::Vector<3> true_direction, const double error_standard_deviation_rad);

const Dynamics* dynamics_; //!< Spacecraft dynamics information
};
Expand Down
14 changes: 7 additions & 7 deletions src/components/ideal/orbit_observer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
#include <setting_file_reader/initialize_file_access.hpp>

OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame,
const libra::Vector<6> error_standard_deviation, const Orbit& orbit)
const math::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());
Expand All @@ -20,11 +20,11 @@ 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();
math::Vector<3> position_error_i_m{0.0};
math::Vector<3> position_error_rtn_m{0.0};
math::Vector<3> velocity_error_i_m_s{0.0};
math::Vector<3> velocity_error_rtn_m_s{0.0};
math::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh();
switch (noise_frame_) {
case NoiseFrame::kInertial:
for (size_t axis = 0; axis < 3; axis++) {
Expand Down Expand Up @@ -93,7 +93,7 @@ OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std

// Noise
const NoiseFrame noise_frame = SetNoiseFrame(ini_file.ReadString("ORBIT_OBSERVER", "noise_frame"));
libra::Vector<6> noise_standard_deviation;
math::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);
Expand Down
Loading

0 comments on commit 768107b

Please sign in to comment.