diff --git a/MIDAS/src/gnc/aero_coeff.h b/MIDAS/src/gnc/aero_coeff.h new file mode 100644 index 00000000..5afb17a2 --- /dev/null +++ b/MIDAS/src/gnc/aero_coeff.h @@ -0,0 +1,125 @@ +/** + * This file contains all the aerodynamic constants + * aero_data - the aerodynamic coefficients for drag calculations + * thurst curves for both motors + * + */ +typedef struct +{ + float mach; + float alpha; + float CA_power_on; + float CN; + float CP; +} aero_coeff_t; + +// stores the aerodynamic coefficients for the corresponding Mach number +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}, + {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.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.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} +}; + +// 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 new file mode 100644 index 00000000..0b82bbc2 --- /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 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 c158c869..b185f5aa 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -1,129 +1,19 @@ #include "ekf.h" #include "finite-state-machines/fsm_states.h" +extern const std::map O5500X_data; +extern const std::map M685W_data; +extern const std::map> motor_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}}; +/** + * 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 @@ -180,6 +70,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; @@ -239,140 +130,81 @@ void EKF::initialize(RocketSystems *args) void EKF::priori(float dt, Orientation &orientation, FSMState fsm) { Eigen::Matrix xdot = Eigen::Matrix::Zero(); - Velocity omega = orientation.getVelocity(); - euler_t angles = orientation.getEuler(); - // Eigen::Matrix gravity = Eigen::Matrix::Zero(); + + // angular states from sensors + 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(); if ((fsm > FSMState::STATE_IDLE) && (fsm < FSMState::STATE_LANDED)) { - gravity(0, 0) = -9.81; + Fg_global(0, 0) = -gravity_ms2; } else { - gravity(0, 0) = 0; + Fg_global(0, 0) = 0; } - float m = mass_sustainer; - float h = height_sustainer; + + // mass and height init + 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; + // 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 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); - - float velocity_magnitude = pow(vel_mag_squared, 0.5); - float mach = velocity_magnitude / 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; - float Fax = -0.5 * rho * (vel_mag_squared) * float(Ca) * (pi * r * r); - float Fay = 0; - float Faz = 0; - - Eigen::Matrix Fg_body; - EKF::GlobalToBody(angles, Fg_body); + // 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 - float Fgx = gravity(0, 0); - float Fgy = gravity(1, 0); - float Fgz = gravity(2, 0); + // force due to gravity + Eigen::Matrix Fg_body = Fg_global; + GlobalToBody(angles_rad, Fg_body); - Eigen::Matrix T; + float Fgx = Fg_body(0, 0); + float Fgy = Fg_body(1, 0); + float Fgz = Fg_body(2, 0); - EKF::getThrust(stage_timestamp, angles, fsm, T); + // thurst force + Eigen::Matrix Ft_global; + EKF::getThrust(stage_timestamp, angles_rad, fsm, Ft_global); - float Ftx = T(0, 0); - float Fty = T(1, 0); - float Ftz = T(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) / 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 - (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) / 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 - (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) / 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 - (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, w_x, w_y, w_z); + setF(dt, omega_rps.vx, omega_rps.vy, omega_rps.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) - { - 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) - { - // 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 * @@ -383,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; @@ -395,44 +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_g = Eigen::Matrix(Eigen::Matrix::Zero()); - (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_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; + euler_t angles_rad = orientation.getEuler(); + angles_rad.yaw = -angles_rad.yaw; // coordinate frame match - Eigen::Matrix acc; - EKF::BodyToGlobal(angles, accel, acc); - float g; + BodyToGlobal(angles_rad, sensor_accel_global_g); + + float g_ms2; if ((FSM_state > FSMState::STATE_IDLE) && (FSM_state < FSMState::STATE_LANDED)) { - g = -9.81; + g_ms2 = -gravity_ms2; } else { - g = 0; + g_ms2 = 0; } - 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_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; - alt_buffer.push(barometer.altitude); + y_k(0, 0) = barometer.altitude; // meters // # Posteriori Update x_k = x_priori + K * (y_k - (H * x_priori)); @@ -552,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. * @@ -620,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 235dfdf6..d8530115 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -3,11 +3,17 @@ #include "kalman_filter.h" #include "sensor_data.h" #include "Buffer.h" +#include "constants.h" +#include "aero_coeff.h" +#include "rotation.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: @@ -19,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); @@ -38,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..543d1cde --- /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_rad, Eigen::Matrix &body_vec) +{ + Eigen::Matrix3f roll, pitch, yaw; + roll << 1., 0., 0., + 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_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; + 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_rad, Eigen::Matrix &global_vec) +{ + Eigen::Matrix3f roll, pitch, yaw; + roll << 1, 0, 0, + 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_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; + Eigen::Vector3f temp = rotation_matrix.transpose() * global_vec; + global_vec = temp; +}