From 2f47e16664462f004b1c548d85c8fa39f7401247 Mon Sep 17 00:00:00 2001 From: Shishir Date: Mon, 6 Oct 2025 19:56:49 -0500 Subject: [PATCH 1/3] reorg of constants --- MIDAS/src/gnc/aero_coeff.h | 105 ++++++++++++++++++++++ MIDAS/src/gnc/constants.h | 10 +++ MIDAS/src/gnc/ekf.cpp | 179 ++++++++----------------------------- MIDAS/src/gnc/ekf.h | 5 ++ 4 files changed, 158 insertions(+), 141 deletions(-) create mode 100644 MIDAS/src/gnc/aero_coeff.h create mode 100644 MIDAS/src/gnc/constants.h diff --git a/MIDAS/src/gnc/aero_coeff.h b/MIDAS/src/gnc/aero_coeff.h new file mode 100644 index 00000000..eaf79b6d --- /dev/null +++ b/MIDAS/src/gnc/aero_coeff.h @@ -0,0 +1,105 @@ +typedef struct +{ + float mach; + float alpha; + float CA_power_on; + float CN; + float CP; +} AeroCoeff; + +// stores the aerodynamic coefficients for the corresponding Mach number +const AeroCoeff aero_data[] = { + {0.04, 0, 1.000001789, 25.80486518, 123.856999}, + {0.08, 0, 0.899149955, 25.80486518, 123.856999}, + {0.12, 0, 0.848262793, 25.80486518, 123.856999}, + {0.16, 0, 0.815490497, 25.80486518, 123.856999}, + {0.2, 0, 0.791977907, 25.80486518, 123.856999}, + {0.24, 0, 0.77407759, 25.80486518, 123.856999}, + {0.28, 0, 0.763046286, 25.80486518, 123.856999}, + {0.32, 0, 0.758319846, 25.80486518, 123.856999}, + {0.36, 0, 0.760511343, 25.80486518, 123.856999}, + {0.4, 0, 0.763737136, 25.80486518, 123.856999}, + {0.44, 0, 0.767325178, 25.80486518, 123.856999}, + {0.48, 0, 0.771334851, 25.80486518, 123.856999}, + {0.52, 0, 0.775843406, 25.80486518, 123.856999}, + {0.56, 0, 0.780953377, 25.80486518, 123.856999}, + {0.6, 0, 0.785771581, 25.80486518, 123.856999}, + {0.64, 0, 0.793730593, 25.80486518, 123.856999}, + {0.68, 0, 0.80285965, 25.80486518, 123.856999}, + {0.72, 0, 0.807910063, 25.80486518, 123.856999}, + {0.76, 0, 0.807403195, 25.80486518, 123.856999}, + {0.8, 0, 0.806889479, 25.80486518, 123.856999}, + {0.84, 0, 0.832707826, 25.80486518, 123.856999}, + {0.88, 0, 0.858519521, 25.80486518, 123.856999}, + {0.92, 0, 0.895125486, 25.492166, 124.3408619}, + {0.96, 0, 0.923744595, 24.86676763, 125.3085876}, + {1, 0, 0.941214699, 24.24136926, 126.2763132}, +}; + +// Moonburner motor thrust curve (Sustainer) +const std::map M685W_data = { + {0.083, 1333.469}, + {0.13, 1368.376}, + {0.249, 1361.395}, + {0.308, 1380.012}, + {0.403, 1359.068}, + {0.675, 1184.53}, + {1.018, 1072.826}, + {1.456, 996.029}, + {1.977, 958.794}, + {2.995, 914.578}, + {3.99, 856.399}, + {4.985, 781.929}, + {5.494, 730.732}, + {5.991, 679.534}, + {7.258, 542.231}, + {7.862, 463.107}, + {8.015, 456.125}, + {8.998, 330.458}, + {9.993, 207.118}, + {10.514, 137.303}, + {11.496, 34.908}, + {11.994, 0.0}}; + +// O5500X motor thrust curve (Booster) +const std::map O5500X_data = { + {0.009, 20.408}, + {0.044, 7112.245}, + {0.063, 6734.694}, + {0.078, 6897.959}, + {0.094, 6612.245}, + {0.109, 6765.306}, + {0.125, 6540.816}, + {0.147, 6581.633}, + {0.194, 6520.408}, + {0.35, 6795.918}, + {0.428, 7091.837}, + {0.563, 7285.714}, + {0.694, 7408.163}, + {0.988, 7581.633}, + {1.266, 7622.449}, + {1.491, 7724.49}, + {1.581, 7653.061}, + {1.641, 7540.816}, + {1.684, 7500.0}, + {1.716, 7336.735}, + {1.784, 7224.49}, + {1.938, 6785.714}, + {2.138, 6326.531}, + {2.491, 5897.959}, + {2.6, 5704.082}, + {2.919, 3540.816}, + {3.022, 3408.163}, + {3.138, 2887.755}, + {3.3, 2234.694}, + {3.388, 1673.469}, + {3.441, 1489.796}, + {3.544, 1418.367}, + {3.609, 1295.918}, + {3.688, 816.327}, + {3.778, 653.061}, + {3.819, 581.633}, + {3.853, 489.796}, + {3.897, 285.714}, + {3.981, 20.408}, + {3.997, 0.0}}; diff --git a/MIDAS/src/gnc/constants.h b/MIDAS/src/gnc/constants.h new file mode 100644 index 00000000..fc96ecab --- /dev/null +++ b/MIDAS/src/gnc/constants.h @@ -0,0 +1,10 @@ +// constants +const float pi = 3.14159268; +const float a = 343.0; // (m/s) speed of sound +const float rho = 1.225; // average air density +const float r = 0.03935; // (m) +const float height_full = 4.457; // (m) height of rocket Full Stage +const float height_sustainer = 2.029; // (m) height of rocket Sustainer +const float mass_full = 33.84; // (kg) Sustainer + Booster +const float mass_sustainer = 10.93; // (kg) Sustainer +const float grav_ms = 9.81; // (m/s) accel due to gravity diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index c158c869..2f4ae308 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -1,130 +1,14 @@ #include "ekf.h" #include "finite-state-machines/fsm_states.h" +extern const std::map O5500X_data; +extern const std::map M685W_data; + EKF::EKF() : KalmanFilter() { state = KalmanData(); } -// constants -const float pi = 3.14159268; -const float a = 343.0; // (m/s) speed of sound -const float rho = 1.225; // average air density -const float r = 0.03935; // (m) -const float height_full = 4.457; // (m) height of rocket Full Stage -const float height_sustainer = 2.029; // (m) height of rocket Sustainer -const float mass_full = 33.84; // (kg) Sustainer + Booster -const float mass_sustainer = 10.93; // (kg) Sustainer - -typedef struct -{ - float mach; - float alpha; - float CA_power_on; - float CN; - float CP; -} AeroCoeff; - -// stores the aerodynamic coefficients for the corresponding Mach number -const AeroCoeff aero_data[] = { - {0.04, 0, 1.000001789, 25.80486518, 123.856999}, - {0.08, 0, 0.899149955, 25.80486518, 123.856999}, - {0.12, 0, 0.848262793, 25.80486518, 123.856999}, - {0.16, 0, 0.815490497, 25.80486518, 123.856999}, - {0.2, 0, 0.791977907, 25.80486518, 123.856999}, - {0.24, 0, 0.77407759, 25.80486518, 123.856999}, - {0.28, 0, 0.763046286, 25.80486518, 123.856999}, - {0.32, 0, 0.758319846, 25.80486518, 123.856999}, - {0.36, 0, 0.760511343, 25.80486518, 123.856999}, - {0.4, 0, 0.763737136, 25.80486518, 123.856999}, - {0.44, 0, 0.767325178, 25.80486518, 123.856999}, - {0.48, 0, 0.771334851, 25.80486518, 123.856999}, - {0.52, 0, 0.775843406, 25.80486518, 123.856999}, - {0.56, 0, 0.780953377, 25.80486518, 123.856999}, - {0.6, 0, 0.785771581, 25.80486518, 123.856999}, - {0.64, 0, 0.793730593, 25.80486518, 123.856999}, - {0.68, 0, 0.80285965, 25.80486518, 123.856999}, - {0.72, 0, 0.807910063, 25.80486518, 123.856999}, - {0.76, 0, 0.807403195, 25.80486518, 123.856999}, - {0.8, 0, 0.806889479, 25.80486518, 123.856999}, - {0.84, 0, 0.832707826, 25.80486518, 123.856999}, - {0.88, 0, 0.858519521, 25.80486518, 123.856999}, - {0.92, 0, 0.895125486, 25.492166, 124.3408619}, - {0.96, 0, 0.923744595, 24.86676763, 125.3085876}, - {1, 0, 0.941214699, 24.24136926, 126.2763132}, -}; - -// Number of entries -#define AERO_DATA_SIZE (sizeof(aero_data) / sizeof(aero_data[0])) - -// Moonburner motor thrust curve (Sustainer) -const std::map M685W_data = { - {0.083, 1333.469}, - {0.13, 1368.376}, - {0.249, 1361.395}, - {0.308, 1380.012}, - {0.403, 1359.068}, - {0.675, 1184.53}, - {1.018, 1072.826}, - {1.456, 996.029}, - {1.977, 958.794}, - {2.995, 914.578}, - {3.99, 856.399}, - {4.985, 781.929}, - {5.494, 730.732}, - {5.991, 679.534}, - {7.258, 542.231}, - {7.862, 463.107}, - {8.015, 456.125}, - {8.998, 330.458}, - {9.993, 207.118}, - {10.514, 137.303}, - {11.496, 34.908}, - {11.994, 0.0}}; - -// O5500X motor thrust curve (Booster) -std::map O5500X_data = { - {0.009, 20.408}, - {0.044, 7112.245}, - {0.063, 6734.694}, - {0.078, 6897.959}, - {0.094, 6612.245}, - {0.109, 6765.306}, - {0.125, 6540.816}, - {0.147, 6581.633}, - {0.194, 6520.408}, - {0.35, 6795.918}, - {0.428, 7091.837}, - {0.563, 7285.714}, - {0.694, 7408.163}, - {0.988, 7581.633}, - {1.266, 7622.449}, - {1.491, 7724.49}, - {1.581, 7653.061}, - {1.641, 7540.816}, - {1.684, 7500.0}, - {1.716, 7336.735}, - {1.784, 7224.49}, - {1.938, 6785.714}, - {2.138, 6326.531}, - {2.491, 5897.959}, - {2.6, 5704.082}, - {2.919, 3540.816}, - {3.022, 3408.163}, - {3.138, 2887.755}, - {3.3, 2234.694}, - {3.388, 1673.469}, - {3.441, 1489.796}, - {3.544, 1418.367}, - {3.609, 1295.918}, - {3.688, 816.327}, - {3.778, 653.061}, - {3.819, 581.633}, - {3.853, 489.796}, - {3.897, 285.714}, - {3.981, 20.408}, - {3.997, 0.0}}; - /** * @brief Sets altitude by averaging 30 barometer measurements taken 100 ms * apart @@ -180,6 +64,7 @@ void EKF::initialize(RocketSystems *args) F_mat.setZero(); // Initialize with zeros + // Initialize Q from filterpy Q(0, 0) = pow(s_dt, 5) / 20; Q(0, 1) = pow(s_dt, 4) / 8; Q(0, 2) = pow(s_dt, 3) / 6; @@ -244,37 +129,43 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) // Eigen::Matrix gravity = Eigen::Matrix::Zero(); if ((fsm > FSMState::STATE_IDLE) && (fsm < FSMState::STATE_LANDED)) { - gravity(0, 0) = -9.81; + gravity(0, 0) = -grav_ms; } else { gravity(0, 0) = 0; } - float m = mass_sustainer; - float h = height_sustainer; + + // mass and height initialization + float curr_mass_kg = mass_sustainer; + float curr_height_m = height_sustainer; if (fsm < FSMState::STATE_BURNOUT) { - m = mass_full; - h = height_full; + curr_mass_kg = mass_full; + curr_height_m = height_full; } float w_x = omega.vx; float w_y = omega.vy; float w_z = omega.vz; - float vel_mag_squared = x_k(1, 0) * x_k(1, 0) + x_k(4, 0) * x_k(4, 0) + x_k(7, 0) * x_k(7, 0); + // finding the Mach number + float vel_mag_squared_ms = x_k(1, 0) * x_k(1, 0) + x_k(4, 0) * x_k(4, 0) + x_k(7, 0) * x_k(7, 0); + float vel_magnitude_ms = pow(vel_mag_squared_ms, 0.5); + float mach = vel_magnitude_ms / a; - float velocity_magnitude = pow(vel_mag_squared, 0.5); - float mach = velocity_magnitude / a; + // finding the closest aerodynamic coeff. C_a int index = std::round(mach / 0.04); index = std::clamp(index, 0, (int)AERO_DATA_SIZE - 1); Ca = aero_data[index].CA_power_on; - float Fax = -0.5 * rho * (vel_mag_squared) * float(Ca) * (pi * r * r); - float Fay = 0; - float Faz = 0; + // aerodynamic force + float Fax = -0.5 * rho * (vel_mag_squared_ms) * float(Ca) * (pi * r * r); + float Fay = 0; // assuming no aerodynamic effects + float Faz = 0; // assuming no aerodynamic effects + // force due to gravity Eigen::Matrix Fg_body; EKF::GlobalToBody(angles, Fg_body); @@ -282,25 +173,28 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) float Fgy = gravity(1, 0); float Fgz = gravity(2, 0); - Eigen::Matrix T; + // thurst force + Eigen::Matrix thrust_vec; - EKF::getThrust(stage_timestamp, angles, fsm, T); + EKF::getThrust(stage_timestamp, angles, fsm, thrust_vec); - float Ftx = T(0, 0); - float Fty = T(1, 0); - float Ftz = T(2, 0); + float Ftx = thrust_vec(0, 0); + float Fty = thrust_vec(1, 0); + float Ftz = thrust_vec(2, 0); xdot << x_k(1, 0), - ((Fax + Ftx + Fgx) / m - (w_y * x_k(7, 0) - w_z * x_k(4, 0)) + x_k(2, 0)) * 0.5, + ((Fax + Ftx + Fgx) / curr_mass_kg - (w_y * x_k(7, 0) - w_z * x_k(4, 0)) + x_k(2, 0)) * 0.5, 0.0, x_k(4, 0), - ((Fay + Fty + Fgy) / m - (w_z * x_k(1, 0) - w_x * x_k(7, 0)) + x_k(5, 0)) * 0.5, + ((Fay + Fty + Fgy) / curr_mass_kg - (w_z * x_k(1, 0) - w_x * x_k(7, 0)) + x_k(5, 0)) * 0.5, 0.0, x_k(7, 0), - ((Faz + Ftz + Fgz) / m - (w_x * x_k(4, 0) - w_y * x_k(1, 0)) + x_k(8, 0)) * 0.5, + ((Faz + Ftz + Fgz) / curr_mass_kg - (w_x * x_k(4, 0) - w_y * x_k(1, 0)) + x_k(8, 0)) * 0.5, 0.0; + + // priori step x_priori = (xdot * dt) + x_k; setF(dt, w_x, w_y, w_z); P_priori = (F_mat * P_k * F_mat.transpose()) + Q; @@ -336,7 +230,7 @@ void EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state, Eigen:: if (FSM_state < FSMState::STATE_BURNOUT) { // first stage - if (timestamp >= 0.009) + if (timestamp >= 0.009) // TODO: index the first value from the correct motor { auto it = O5500X_data.lower_bound(timestamp); if (it != O5500X_data.end()) @@ -352,7 +246,7 @@ void EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state, Eigen:: } else { - if (timestamp >= 0.083) + if (timestamp >= 0.083) // TODO: index the first value from the correct motor { // second stage auto it = M685W_data.lower_bound(timestamp); @@ -408,6 +302,7 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori // Sensor Measurements Eigen::Matrix accel = Eigen::Matrix(Eigen::Matrix::Zero()); + // TODO: document this right (accel)(0, 0) = acceleration.az - 0.045; (accel)(1, 0) = acceleration.ay - 0.065; (accel)(2, 0) = -acceleration.ax - 0.06; @@ -417,16 +312,18 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori Eigen::Matrix acc; EKF::BodyToGlobal(angles, accel, acc); + float g; if ((FSM_state > FSMState::STATE_IDLE) && (FSM_state < FSMState::STATE_LANDED)) { - g = -9.81; + g = -grav_ms; } else { g = 0; } + // TODO: magic numbers y_k(1, 0) = ((acc)(0)) * 9.81 + g; y_k(2, 0) = ((acc)(1)) * 9.81; y_k(3, 0) = ((acc)(2)) * 9.81; diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index 235dfdf6..da74e345 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -3,11 +3,16 @@ #include "kalman_filter.h" #include "sensor_data.h" #include "Buffer.h" +#include "constants.h" +#include "aero_coeff.h" #define NUM_STATES 9 #define NUM_SENSOR_INPUTS 4 #define ALTITUDE_BUFFER_SIZE 10 +// Number of entries for aerodynamic data table +#define AERO_DATA_SIZE (sizeof(aero_data) / sizeof(aero_data[0])) + class EKF : public KalmanFilter { public: From d42030cb433e14dfecdf2776028f6022bc198a05 Mon Sep 17 00:00:00 2001 From: Shishir Date: Tue, 7 Oct 2025 12:28:33 -0500 Subject: [PATCH 2/3] joe flacco to the bengals --- MIDAS/src/gnc/aero_coeff.h | 32 ++++- MIDAS/src/gnc/constants.h | 2 +- MIDAS/src/gnc/ekf.cpp | 246 +++++++++++++++---------------------- MIDAS/src/gnc/ekf.h | 11 +- MIDAS/src/gnc/rotation.h | 55 +++++++++ 5 files changed, 185 insertions(+), 161 deletions(-) create mode 100644 MIDAS/src/gnc/rotation.h diff --git a/MIDAS/src/gnc/aero_coeff.h b/MIDAS/src/gnc/aero_coeff.h index eaf79b6d..5afb17a2 100644 --- a/MIDAS/src/gnc/aero_coeff.h +++ b/MIDAS/src/gnc/aero_coeff.h @@ -1,3 +1,9 @@ +/** + * This file contains all the aerodynamic constants + * aero_data - the aerodynamic coefficients for drag calculations + * thurst curves for both motors + * + */ typedef struct { float mach; @@ -5,10 +11,10 @@ typedef struct float CA_power_on; float CN; float CP; -} AeroCoeff; +} aero_coeff_t; // stores the aerodynamic coefficients for the corresponding Mach number -const AeroCoeff aero_data[] = { +const aero_coeff_t aero_data[] = { {0.04, 0, 1.000001789, 25.80486518, 123.856999}, {0.08, 0, 0.899149955, 25.80486518, 123.856999}, {0.12, 0, 0.848262793, 25.80486518, 123.856999}, @@ -38,7 +44,6 @@ const AeroCoeff aero_data[] = { // Moonburner motor thrust curve (Sustainer) const std::map M685W_data = { - {0.083, 1333.469}, {0.13, 1368.376}, {0.249, 1361.395}, {0.308, 1380.012}, @@ -59,11 +64,11 @@ const std::map M685W_data = { {9.993, 207.118}, {10.514, 137.303}, {11.496, 34.908}, - {11.994, 0.0}}; + {11.994, 0.0} +}; // O5500X motor thrust curve (Booster) const std::map O5500X_data = { - {0.009, 20.408}, {0.044, 7112.245}, {0.063, 6734.694}, {0.078, 6897.959}, @@ -102,4 +107,19 @@ const std::map O5500X_data = { {3.853, 489.796}, {3.897, 285.714}, {3.981, 20.408}, - {3.997, 0.0}}; + {3.997, 0.0} +}; + +// constant variable that contains the booster and sustainer motors +const std::map> motor_data = { + {"Booster", O5500X_data}, + {"Sustainer", M685W_data} +}; + +/** + * @brief linearly interpolates the a value based on the lower and upper bound, similar to lerp_() in PySim + */ +inline float linearInterpolation(float x0, float y0, float x1, float y1, float x) +{ + return y0 + ((x - x0) * (y1 - y0) / (x1 - x0)); +} diff --git a/MIDAS/src/gnc/constants.h b/MIDAS/src/gnc/constants.h index fc96ecab..0b82bbc2 100644 --- a/MIDAS/src/gnc/constants.h +++ b/MIDAS/src/gnc/constants.h @@ -7,4 +7,4 @@ const float height_full = 4.457; // (m) height of rocket Full Stage const float height_sustainer = 2.029; // (m) height of rocket Sustainer const float mass_full = 33.84; // (kg) Sustainer + Booster const float mass_sustainer = 10.93; // (kg) Sustainer -const float grav_ms = 9.81; // (m/s) accel due to gravity +const float gravity_ms2 = 9.81; // (m/s^2) accel due to gravity diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index 2f4ae308..50abf55d 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -3,12 +3,18 @@ extern const std::map O5500X_data; extern const std::map M685W_data; +extern const std::map> motor_data; EKF::EKF() : KalmanFilter() { state = KalmanData(); } +/** + * THIS IS A PLACEHOLDER FUNCTION SO WE CAN ABSTRACT FROM `kalman_filter.h` + */ +void EKF::priori() {}; + /** * @brief Sets altitude by averaging 30 barometer measurements taken 100 ms * apart @@ -124,19 +130,23 @@ void EKF::initialize(RocketSystems *args) void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { Eigen::Matrix xdot = Eigen::Matrix::Zero(); + + // angular states from sensors Velocity omega = orientation.getVelocity(); euler_t angles = orientation.getEuler(); - // Eigen::Matrix gravity = Eigen::Matrix::Zero(); + + // ignore effects of gravity when on pad + Eigen::Matrix Fg_global = Eigen::Matrix::Zero(); if ((fsm > FSMState::STATE_IDLE) && (fsm < FSMState::STATE_LANDED)) { - gravity(0, 0) = -grav_ms; + Fg_global(0, 0) = -gravity_ms2; } else { - gravity(0, 0) = 0; + Fg_global(0, 0) = 0; } - // mass and height initialization + // mass and height init float curr_mass_kg = mass_sustainer; float curr_height_m = height_sustainer; @@ -146,16 +156,12 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) curr_height_m = height_full; } - float w_x = omega.vx; - float w_y = omega.vy; - float w_z = omega.vz; - - // finding the Mach number + // Mach number float vel_mag_squared_ms = x_k(1, 0) * x_k(1, 0) + x_k(4, 0) * x_k(4, 0) + x_k(7, 0) * x_k(7, 0); float vel_magnitude_ms = pow(vel_mag_squared_ms, 0.5); float mach = vel_magnitude_ms / a; - // finding the closest aerodynamic coeff. C_a + // approximating C_a (aerodynamic coeff.) int index = std::round(mach / 0.04); index = std::clamp(index, 0, (int)AERO_DATA_SIZE - 1); Ca = aero_data[index].CA_power_on; @@ -166,107 +172,39 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) float Faz = 0; // assuming no aerodynamic effects // force due to gravity - Eigen::Matrix Fg_body; - EKF::GlobalToBody(angles, Fg_body); + Eigen::Matrix Fg_body = Fg_global; + GlobalToBody(angles, Fg_body); - float Fgx = gravity(0, 0); - float Fgy = gravity(1, 0); - float Fgz = gravity(2, 0); + float Fgx = Fg_body(0, 0); + float Fgy = Fg_body(1, 0); + float Fgz = Fg_body(2, 0); // thurst force - Eigen::Matrix thrust_vec; + Eigen::Matrix Ft_global; + EKF::getThrust(stage_timestamp, angles, fsm, Ft_global); - EKF::getThrust(stage_timestamp, angles, fsm, thrust_vec); - - float Ftx = thrust_vec(0, 0); - float Fty = thrust_vec(1, 0); - float Ftz = thrust_vec(2, 0); + float Ftx = Ft_global(0, 0); + float Fty = Ft_global(1, 0); + float Ftz = Ft_global(2, 0); xdot << x_k(1, 0), - ((Fax + Ftx + Fgx) / curr_mass_kg - (w_y * x_k(7, 0) - w_z * x_k(4, 0)) + x_k(2, 0)) * 0.5, + ((Fax + Ftx + Fgx) / curr_mass_kg - (omega.vy * x_k(7, 0) - omega.vz * x_k(4, 0)) + x_k(2, 0)) * 0.5, 0.0, x_k(4, 0), - ((Fay + Fty + Fgy) / curr_mass_kg - (w_z * x_k(1, 0) - w_x * x_k(7, 0)) + x_k(5, 0)) * 0.5, + ((Fay + Fty + Fgy) / curr_mass_kg - (omega.vz * x_k(1, 0) - omega.vx * x_k(7, 0)) + x_k(5, 0)) * 0.5, 0.0, x_k(7, 0), - ((Faz + Ftz + Fgz) / curr_mass_kg - (w_x * x_k(4, 0) - w_y * x_k(1, 0)) + x_k(8, 0)) * 0.5, + ((Faz + Ftz + Fgz) / curr_mass_kg - (omega.vx * x_k(4, 0) - omega.vy * x_k(1, 0)) + x_k(8, 0)) * 0.5, 0.0; // priori step x_priori = (xdot * dt) + x_k; - setF(dt, w_x, w_y, w_z); + setF(dt, omega.vx, omega.vy, omega.vz); P_priori = (F_mat * P_k * F_mat.transpose()) + Q; } -/** - * @brief linearly interpolates the a value based on the lower and upper bound, similar to lerp_() in PySim - */ -float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x) -{ - return y0 + ((x - x0) * (y1 - y0) / (x1 - x0)); -} - -/** - * @brief Returns the approximate thrust force from the motor given the thurst curve - * - * @param timestamp Time since most recent ignition - * @param angles Current orientation of the rocket - * @param FSM_state Current FSM state - * - * @return Thrust force in the body frame - * - * The thrust force is calculated by interpolating the thrust curve data which is stored in an ordered map (see top of file). - * The thrust curve data is different for the booster and sustainer stages, so the function checks the FSM state to determine - * which thrust curve to use. The time since ignition is also important to consider so that is reset once we reach a new stage. - * The thrust force is then rotated into the body frame using the BodyToGlobal function. - */ -void EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state, Eigen::Matrix &to_modify) -{ - float interpolatedValue = 0; - if (FSM_state >= STATE_FIRST_BOOST) - { - if (FSM_state < FSMState::STATE_BURNOUT) - { - // first stage - if (timestamp >= 0.009) // TODO: index the first value from the correct motor - { - auto it = O5500X_data.lower_bound(timestamp); - if (it != O5500X_data.end()) - { - float x0 = it->first; - float y0 = it->second; - ++it; - float x1 = it->first; - float y1 = it->second; - interpolatedValue = linearInterpolation(x0, y0, x1, y1, timestamp); - } - } - } - else - { - if (timestamp >= 0.083) // TODO: index the first value from the correct motor - { - // second stage - auto it = M685W_data.lower_bound(timestamp); - if (it != M685W_data.end()) - { - float x0 = it->first; - float y0 = it->second; - ++it; - float x1 = it->first; - float y1 = it->second; - interpolatedValue = linearInterpolation(x0, y0, x1, y1, timestamp); - } - } - } - } - Eigen::Matrix interpolatedVector = Eigen::Matrix::Zero(); - (interpolatedVector)(0, 0) = interpolatedValue; - EKF::BodyToGlobal(angles, interpolatedVector, to_modify); -} - /** * @brief Update Kalman Gain and state estimate with current sensor data * @@ -277,6 +215,7 @@ void EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state, Eigen:: */ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state) { + // if on pad -> take last 10 barometer measurements for init state if (FSM_state == FSMState::STATE_IDLE) { float sum = 0; @@ -289,47 +228,47 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori KalmanState kalman_state = (KalmanState){sum / 10.0f, 0, 0, 0, 0, 0, 0, 0, 0}; setState(kalman_state); } + // ignore alitiude measurements after apogee else if (FSM_state >= FSMState::STATE_APOGEE) { H(1, 2) = 0; } + // Kalman Gain Eigen::Matrix S_k = Eigen::Matrix::Zero(); S_k = (((H * P_priori * H.transpose()) + R)).inverse(); Eigen::Matrix identity = Eigen::Matrix::Identity(); K = (P_priori * H.transpose()) * S_k; // Sensor Measurements - Eigen::Matrix accel = Eigen::Matrix(Eigen::Matrix::Zero()); + Eigen::Matrix sensor_accel_global = Eigen::Matrix(Eigen::Matrix::Zero()); - // TODO: document this right - (accel)(0, 0) = acceleration.az - 0.045; - (accel)(1, 0) = acceleration.ay - 0.065; - (accel)(2, 0) = -acceleration.ax - 0.06; + // accouting for sensor bias and coordinate frame transforms + (sensor_accel_global)(0, 0) = acceleration.az - 0.045; + (sensor_accel_global)(1, 0) = acceleration.ay - 0.065; + (sensor_accel_global)(2, 0) = -acceleration.ax - 0.06; euler_t angles = orientation.getEuler(); - angles.yaw = -angles.yaw; + angles.yaw = -angles.yaw; // coordinate frame match - Eigen::Matrix acc; - EKF::BodyToGlobal(angles, accel, acc); + BodyToGlobal(angles, sensor_accel_global); float g; if ((FSM_state > FSMState::STATE_IDLE) && (FSM_state < FSMState::STATE_LANDED)) { - g = -grav_ms; + g = -gravity_ms2; } else { g = 0; } - // TODO: magic numbers - y_k(1, 0) = ((acc)(0)) * 9.81 + g; - y_k(2, 0) = ((acc)(1)) * 9.81; - y_k(3, 0) = ((acc)(2)) * 9.81; + // acceloremeter reports values in g's and measures specific force + y_k(1, 0) = ((sensor_accel_global)(0)) * gravity_ms2 + g; + y_k(2, 0) = ((sensor_accel_global)(1)) * gravity_ms2; + y_k(3, 0) = ((sensor_accel_global)(2)) * gravity_ms2; y_k(0, 0) = barometer.altitude; - alt_buffer.push(barometer.altitude); // # Posteriori Update x_k = x_priori + K * (y_k - (H * x_priori)); @@ -449,49 +388,6 @@ void EKF:: Q *= sd; } -/** - * @brief Converts a vector in the body frame to the global frame - * - * @param angles Roll, pitch, yaw angles - * @param body_vect Vector for rotation in the body frame - * @return Eigen::Matrix Rotated vector in the global frame - */ -void EKF::BodyToGlobal(euler_t angles, Eigen::Matrix &body_vect, Eigen::Matrix &to_modify) -{ - Eigen::Matrix3f roll, pitch, yaw; - roll << 1., 0., 0., 0., cos(angles.roll), -sin(angles.roll), 0., sin(angles.roll), cos(angles.roll); - pitch << cos(angles.pitch), 0., sin(angles.pitch), 0., 1., 0., -sin(angles.pitch), 0., cos(angles.pitch); - yaw << cos(angles.yaw), -sin(angles.yaw), 0., sin(angles.yaw), cos(angles.yaw), 0., 0., 0., 1.; - - to_modify = yaw * pitch * roll * (body_vect); -} - -/** - * THIS IS A PLACEHOLDER FUNCTION SO WE CAN ABSTRACT FROM `kalman_filter.h` - */ -void EKF::priori() {}; - -/** - * @brief Converts a vector in the global frame to the body frame - * - * @param angles Roll, pitch, yaw angles - * @param world_vector Vector for rotation in the global frame - * - * @return Eigen::Matrix Rotated vector in the body frame - * TODO: Don't pass in gravity and pass in a vector instead - */ -void EKF::GlobalToBody(euler_t angles, Eigen::Matrix &to_modify) -{ - Eigen::Matrix3f roll; - roll << 1, 0, 0, 0, cos(angles.roll), -sin(angles.roll), 0, sin(angles.roll), cos(angles.roll); - Eigen::Matrix3f pitch; - pitch << cos(angles.pitch), 0, sin(angles.pitch), 0, 1, 0, -sin(angles.pitch), 0, cos(angles.pitch); - Eigen::Matrix3f yaw; - yaw << cos(angles.yaw), -sin(angles.yaw), 0, sin(angles.yaw), cos(angles.yaw), 0, 0, 0, 1; - Eigen::Matrix3f rotation_matrix = yaw * pitch * roll; - to_modify = rotation_matrix.transpose() * gravity; -} - /** * @brief Sets the F matrix given time step. * @@ -517,4 +413,56 @@ void EKF::setF(float dt, float wx, float wy, float wz) F_mat(7, 8) = 0.5; } +/** + * @brief Returns the approximate thrust force from the motor given the thurst curve + * + * @param timestamp Time since most recent ignition + * @param angles Current orientation of the rocket + * @param FSM_state Current FSM state + * + * @return Thrust force in the body frame + * + * The thrust force is calculated by interpolating the thrust curve data which is stored in an ordered map (see top of file). + * The thrust curve data is different for the booster and sustainer stages, so the function checks the FSM state to determine + * which thrust curve to use. The time since ignition is also important to consider so that is reset once we reach a new stage. + * The thrust force is then rotated into the body frame using the BodyToGlobal function. + */ +void EKF::getThrust(float timestamp, const euler_t& angles, FSMState FSM_state, Eigen::Vector3f& thrust_out) +{ + // Pick which motor thrust curve to use + const std::map* thrust_curve = nullptr; + if (FSM_state >= STATE_FIRST_BOOST && FSM_state < FSMState::STATE_BURNOUT) + thrust_curve = &motor_data.at("Booster"); // Booster + else if (FSM_state >= FSMState::STATE_BURNOUT) + thrust_curve = &motor_data.at("Sustainer"); // Sustainer + else { + thrust_out.setZero(); + return; // No thrust before ignition + } + + // Handle case where timestamp is before or after available data + if (timestamp <= thrust_curve->begin()->first) { + thrust_out = Eigen::Vector3f(thrust_curve->begin()->second, 0.f, 0.f); + } + else if (timestamp >= thrust_curve->rbegin()->first) { + thrust_out.setZero(); // assume motor burned out after curve ends + } + else { + // Find interpolation interval + auto it_upper = thrust_curve->lower_bound(timestamp); + auto it_lower = std::prev(it_upper); + + float x0 = it_lower->first; + float y0 = it_lower->second; + float x1 = it_upper->first; + float y1 = it_upper->second; + + float interpolated_thrust = linearInterpolation(x0, y0, x1, y1, timestamp); + thrust_out = Eigen::Vector3f(interpolated_thrust, 0.f, 0.f); + } + + // Rotate from body to global + BodyToGlobal(angles, thrust_out); +} + EKF ekf; diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index da74e345..d8530115 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -5,6 +5,7 @@ #include "Buffer.h" #include "constants.h" #include "aero_coeff.h" +#include "rotation.h" #define NUM_STATES 9 #define NUM_SENSOR_INPUTS 4 @@ -24,14 +25,14 @@ class EKF : public KalmanFilter void setQ(float dt, float sd); void setF(float dt, float w_x, float w_y, float w_z); - void getThrust(float timestamp, euler_t angles, FSMState FSM_state, Eigen::Matrix &to_modify); - void BodyToGlobal(euler_t angles, Eigen::Matrix &x_k, Eigen::Matrix &to_modify); - void GlobalToBody(euler_t angles, Eigen::Matrix &to_modify); + + // void BodyToGlobal(euler_t angles, Eigen::Matrix &body_vec); + // void GlobalToBody(euler_t angles, Eigen::Matrix &global_vec); KalmanData getState() override; void setState(KalmanState state) override; - float linearInterpolation(float x0, float y0, float x1, float y1, float x); + void getThrust(float timestamp, const euler_t& angles, FSMState FSM_state, Eigen::Vector3f& thrust_out); void tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState state); @@ -43,7 +44,7 @@ class EKF : public KalmanFilter float Ca = 0; float Cn = 0; float Cp = 0; - Eigen::Matrix gravity = Eigen::Matrix::Zero(); + // Eigen::Matrix gravity = Eigen::Matrix::Zero(); KalmanState kalman_state; FSMState last_fsm = FSMState::STATE_IDLE; float stage_timestamp = 0; diff --git a/MIDAS/src/gnc/rotation.h b/MIDAS/src/gnc/rotation.h new file mode 100644 index 00000000..9caa5fc5 --- /dev/null +++ b/MIDAS/src/gnc/rotation.h @@ -0,0 +1,55 @@ +#include +/**************************** ROTATION FUNCTIONS ****************************/ + +/** + * @brief Converts a vector in the body frame to the global frame + * + * @param angles Roll, pitch, yaw angles + * @param body_vec Vector for rotation in the body frame + * @return Eigen::Matrix Rotated vector in the global frame + */ +template +void BodyToGlobal(Angles& angles, Eigen::Matrix &body_vec) +{ + Eigen::Matrix3f roll, pitch, yaw; + roll << 1., 0., 0., + 0., cos(angles.roll), -sin(angles.roll), + 0., sin(angles.roll), cos(angles.roll); + pitch << cos(angles.pitch), 0., sin(angles.pitch), + 0., 1., 0., + -sin(angles.pitch), 0., cos(angles.pitch); + yaw << cos(angles.yaw), -sin(angles.yaw), 0., + sin(angles.yaw), cos(angles.yaw), 0., + 0., 0., 1.; + + Eigen::Matrix3f rotation_matrix = yaw * pitch * roll; + Eigen::Vector3f temp = rotation_matrix * body_vec; + body_vec = temp; +} + +/** + * @brief Converts a vector in the global frame to the body frame + * + * @param angles Roll, pitch, yaw angles + * @param global_vec Vector for rotation in the global frame + * + * @return Eigen::Matrix Rotated vector in the body frame + */ +template +void GlobalToBody(Angles& angles, Eigen::Matrix &global_vec) +{ + Eigen::Matrix3f roll, pitch, yaw; + roll << 1, 0, 0, + 0, cos(angles.roll), -sin(angles.roll), + 0, sin(angles.roll), cos(angles.roll); + pitch << cos(angles.pitch), 0, sin(angles.pitch), + 0, 1, 0, + -sin(angles.pitch), 0, cos(angles.pitch); + yaw << cos(angles.yaw), -sin(angles.yaw), 0, + sin(angles.yaw), cos(angles.yaw), + 0, 0, 1; + + Eigen::Matrix3f rotation_matrix = yaw * pitch * roll; + Eigen::Vector3f temp = rotation_matrix.transpose() * global_vec; + global_vec = temp; +} From f9b0d7c30a3c66fa2e2986e0b3a28a1cd6961f7f Mon Sep 17 00:00:00 2001 From: Shishir Date: Tue, 7 Oct 2025 12:46:50 -0500 Subject: [PATCH 3/3] adding units --- MIDAS/src/gnc/ekf.cpp | 44 ++++++++++++++++++++-------------------- MIDAS/src/gnc/rotation.h | 28 ++++++++++++------------- 2 files changed, 36 insertions(+), 36 deletions(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index 50abf55d..b185f5aa 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -132,8 +132,8 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) Eigen::Matrix xdot = Eigen::Matrix::Zero(); // angular states from sensors - Velocity omega = orientation.getVelocity(); - euler_t angles = orientation.getEuler(); + Velocity omega_rps = orientation.getVelocity(); // rads per sec + euler_t angles_rad = orientation.getEuler(); // ignore effects of gravity when on pad Eigen::Matrix Fg_global = Eigen::Matrix::Zero(); @@ -173,7 +173,7 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) // force due to gravity Eigen::Matrix Fg_body = Fg_global; - GlobalToBody(angles, Fg_body); + GlobalToBody(angles_rad, Fg_body); float Fgx = Fg_body(0, 0); float Fgy = Fg_body(1, 0); @@ -181,27 +181,27 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm) // thurst force Eigen::Matrix Ft_global; - EKF::getThrust(stage_timestamp, angles, fsm, Ft_global); + EKF::getThrust(stage_timestamp, angles_rad, fsm, Ft_global); float Ftx = Ft_global(0, 0); float Fty = Ft_global(1, 0); float Ftz = Ft_global(2, 0); xdot << x_k(1, 0), - ((Fax + Ftx + Fgx) / curr_mass_kg - (omega.vy * x_k(7, 0) - omega.vz * x_k(4, 0)) + x_k(2, 0)) * 0.5, + ((Fax + Ftx + Fgx) / curr_mass_kg - (omega_rps.vy * x_k(7, 0) - omega_rps.vz * x_k(4, 0)) + x_k(2, 0)) * 0.5, 0.0, x_k(4, 0), - ((Fay + Fty + Fgy) / curr_mass_kg - (omega.vz * x_k(1, 0) - omega.vx * x_k(7, 0)) + x_k(5, 0)) * 0.5, + ((Fay + Fty + Fgy) / curr_mass_kg - (omega_rps.vz * x_k(1, 0) - omega_rps.vx * x_k(7, 0)) + x_k(5, 0)) * 0.5, 0.0, x_k(7, 0), - ((Faz + Ftz + Fgz) / curr_mass_kg - (omega.vx * x_k(4, 0) - omega.vy * x_k(1, 0)) + x_k(8, 0)) * 0.5, + ((Faz + Ftz + Fgz) / curr_mass_kg - (omega_rps.vx * x_k(4, 0) - omega_rps.vy * x_k(1, 0)) + x_k(8, 0)) * 0.5, 0.0; // priori step x_priori = (xdot * dt) + x_k; - setF(dt, omega.vx, omega.vy, omega.vz); + setF(dt, omega_rps.vx, omega_rps.vy, omega_rps.vz); P_priori = (F_mat * P_k * F_mat.transpose()) + Q; } @@ -241,34 +241,34 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori K = (P_priori * H.transpose()) * S_k; // Sensor Measurements - Eigen::Matrix sensor_accel_global = Eigen::Matrix(Eigen::Matrix::Zero()); + Eigen::Matrix sensor_accel_global_g = Eigen::Matrix(Eigen::Matrix::Zero()); // accouting for sensor bias and coordinate frame transforms - (sensor_accel_global)(0, 0) = acceleration.az - 0.045; - (sensor_accel_global)(1, 0) = acceleration.ay - 0.065; - (sensor_accel_global)(2, 0) = -acceleration.ax - 0.06; + (sensor_accel_global_g)(0, 0) = acceleration.az - 0.045; + (sensor_accel_global_g)(1, 0) = acceleration.ay - 0.065; + (sensor_accel_global_g)(2, 0) = -acceleration.ax - 0.06; - euler_t angles = orientation.getEuler(); - angles.yaw = -angles.yaw; // coordinate frame match + euler_t angles_rad = orientation.getEuler(); + angles_rad.yaw = -angles_rad.yaw; // coordinate frame match - BodyToGlobal(angles, sensor_accel_global); + BodyToGlobal(angles_rad, sensor_accel_global_g); - float g; + float g_ms2; if ((FSM_state > FSMState::STATE_IDLE) && (FSM_state < FSMState::STATE_LANDED)) { - g = -gravity_ms2; + g_ms2 = -gravity_ms2; } else { - g = 0; + g_ms2 = 0; } // acceloremeter reports values in g's and measures specific force - y_k(1, 0) = ((sensor_accel_global)(0)) * gravity_ms2 + g; - y_k(2, 0) = ((sensor_accel_global)(1)) * gravity_ms2; - y_k(3, 0) = ((sensor_accel_global)(2)) * gravity_ms2; + y_k(1, 0) = ((sensor_accel_global_g)(0)) * gravity_ms2 + g_ms2; + y_k(2, 0) = ((sensor_accel_global_g)(1)) * gravity_ms2; + y_k(3, 0) = ((sensor_accel_global_g)(2)) * gravity_ms2; - y_k(0, 0) = barometer.altitude; + y_k(0, 0) = barometer.altitude; // meters // # Posteriori Update x_k = x_priori + K * (y_k - (H * x_priori)); diff --git a/MIDAS/src/gnc/rotation.h b/MIDAS/src/gnc/rotation.h index 9caa5fc5..543d1cde 100644 --- a/MIDAS/src/gnc/rotation.h +++ b/MIDAS/src/gnc/rotation.h @@ -9,17 +9,17 @@ * @return Eigen::Matrix Rotated vector in the global frame */ template -void BodyToGlobal(Angles& angles, Eigen::Matrix &body_vec) +void BodyToGlobal(Angles& angles_rad, Eigen::Matrix &body_vec) { Eigen::Matrix3f roll, pitch, yaw; roll << 1., 0., 0., - 0., cos(angles.roll), -sin(angles.roll), - 0., sin(angles.roll), cos(angles.roll); - pitch << cos(angles.pitch), 0., sin(angles.pitch), + 0., cos(angles_rad.roll), -sin(angles_rad.roll), + 0., sin(angles_rad.roll), cos(angles_rad.roll); + pitch << cos(angles_rad.pitch), 0., sin(angles_rad.pitch), 0., 1., 0., - -sin(angles.pitch), 0., cos(angles.pitch); - yaw << cos(angles.yaw), -sin(angles.yaw), 0., - sin(angles.yaw), cos(angles.yaw), 0., + -sin(angles_rad.pitch), 0., cos(angles_rad.pitch); + yaw << cos(angles_rad.yaw), -sin(angles_rad.yaw), 0., + sin(angles_rad.yaw), cos(angles_rad.yaw), 0., 0., 0., 1.; Eigen::Matrix3f rotation_matrix = yaw * pitch * roll; @@ -36,17 +36,17 @@ void BodyToGlobal(Angles& angles, Eigen::Matrix &body_vec) * @return Eigen::Matrix Rotated vector in the body frame */ template -void GlobalToBody(Angles& angles, Eigen::Matrix &global_vec) +void GlobalToBody(Angles& angles_rad, Eigen::Matrix &global_vec) { Eigen::Matrix3f roll, pitch, yaw; roll << 1, 0, 0, - 0, cos(angles.roll), -sin(angles.roll), - 0, sin(angles.roll), cos(angles.roll); - pitch << cos(angles.pitch), 0, sin(angles.pitch), + 0, cos(angles_rad.roll), -sin(angles_rad.roll), + 0, sin(angles_rad.roll), cos(angles_rad.roll); + pitch << cos(angles_rad.pitch), 0, sin(angles_rad.pitch), 0, 1, 0, - -sin(angles.pitch), 0, cos(angles.pitch); - yaw << cos(angles.yaw), -sin(angles.yaw), 0, - sin(angles.yaw), cos(angles.yaw), + -sin(angles_rad.pitch), 0, cos(angles_rad.pitch); + yaw << cos(angles_rad.yaw), -sin(angles_rad.yaw), 0, + sin(angles_rad.yaw), cos(angles_rad.yaw), 0, 0, 1; Eigen::Matrix3f rotation_matrix = yaw * pitch * roll;