Skip to content

Commit c8fde09

Browse files
committed
Add noise
1 parent 6a899e2 commit c8fde09

File tree

2 files changed

+47
-5
lines changed

2 files changed

+47
-5
lines changed

src/components/ideal/orbit_observer.cpp

Lines changed: 24 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,12 +7,32 @@
77

88
#include <library/initialize/initialize_file_access.hpp>
99

10-
OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit)
11-
: Component(prescaler, clock_generator), orbit_(orbit) {}
10+
OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame,
11+
const libra::Vector<6> error_standard_deviation, const const Orbit& orbit)
12+
: Component(prescaler, clock_generator), orbit_(orbit) {
13+
for (size_t i = 0; i < 6; i++) {
14+
normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i]);
15+
}
16+
}
1217

1318
void OrbitObserver::MainRoutine(const int time_count) {
1419
UNUSED(time_count);
1520

21+
switch (error_frame_) {
22+
case ErrorFrame::kInertial:
23+
observed_position_i_m_ = Measure(observed_position_i_m_);
24+
// Frame conversion
25+
observed_position_rtn_m_ = q_i2rtn.FrameConversion(observed_position_i_m_);
26+
break;
27+
case ErrorFrame::kRtn:
28+
observed_position_rtn_m_ = Measure(observed_position_rtn_m_);
29+
// Frame conversion
30+
observed_position_i_m_ = q_i2rtn.InverseFrameConversion(observed_position_rtn_m_);
31+
break;
32+
default:
33+
break;
34+
}
35+
1636
observed_position_i_m_ = orbit_.GetPosition_i_m();
1737
observed_velocity_i_m_s_ = orbit_.GetVelocity_i_m_s();
1838
}
@@ -36,6 +56,8 @@ std::string OrbitObserver::GetLogValue() const {
3656
return str_tmp;
3757
}
3858

59+
void AddNoise(){}
60+
3961
OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) {
4062
// General
4163
IniAccess ini_file(file_name);

src/components/ideal/orbit_observer.hpp

Lines changed: 23 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,15 @@
1313

1414
#include "../base/component.hpp"
1515

16+
/**
17+
* @enum ErrorFrame
18+
* @brief Error definition frame
19+
*/
20+
enum class ErrorFrame {
21+
kInertial, //!< Inertial frame
22+
kRtn, //!< RTN frame
23+
};
24+
1625
/*
1726
* @class OrbitObserver
1827
* @brief Ideal component which can observe orbit
@@ -24,9 +33,12 @@ class OrbitObserver : public Component, public ILoggable {
2433
* @brief Constructor without power port
2534
* @param [in] prescaler: Frequency scale factor for update
2635
* @param [in] clock_generator: Clock generator
36+
* @param [in] error_frame: Error frame definition
37+
* @param [in] error_standard_deviation: Position and Velocity standard deviation noise [m, m/s]
2738
* @param [in] orbit: Orbit information
2839
*/
29-
OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const Orbit& orbit);
40+
OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const ErrorFrame error_frame, const libra::Vector<6> error_standard_deviation,
41+
const const Orbit& orbit);
3042

3143
/**
3244
* @fn ~AttitudeObserver
@@ -66,11 +78,19 @@ class OrbitObserver : public Component, public ILoggable {
6678
inline const libra::Vector<3> GetVelocity_i_m_s() const { return observed_velocity_i_m_s_; };
6779

6880
protected:
69-
libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m]
70-
libra::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s]
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+
libra::Vector<3> observed_position_rtn_m_{0.0}; //!< Observed position @ RTN frame [m]
84+
libra::Vector<3> observed_velocity_rtn_m_s_{0.0}; //!< Observed velocity @ RTN frame [m/s]
85+
86+
ErrorFrame error_frame_; //!< Error definition frame
87+
libra::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s]
7188

7289
// Observed variables
7390
const Orbit& orbit_; //!< Orbit information
91+
92+
libra::Vector<3> AddPositionNoise(const libra::Vector<3> position_m);
93+
libra::Vector<3> AddVelocityNoise(const libra::Vector<3> velocity_m_s);
7494
};
7595

7696
/**

0 commit comments

Comments
 (0)