From 8a41776e21b6938f4254e38b9a22e293e27c119c Mon Sep 17 00:00:00 2001 From: Divij Date: Mon, 9 Feb 2026 20:01:49 -0600 Subject: [PATCH 1/3] implemented gnc integration for midas mini --- MIDAS/src/gnc/constants.h | 11 +- MIDAS/src/gnc/ekf.cpp | 563 ++++++------------ MIDAS/src/gnc/ekf.h | 50 +- MIDAS/src/gnc/kalman_filter.h | 11 +- MIDAS/src/gnc/mqekf.cpp | 344 +++++++++++ MIDAS/src/gnc/mqekf.h | 62 ++ MIDAS/src/gnc/mqekf_test.cpp | 124 ++++ .../src/gnc/{ => old_filters}/example_kf.cpp | 2 +- MIDAS/src/gnc/{ => old_filters}/example_kf.h | 2 +- MIDAS/src/gnc/{ => old_filters}/yessir.cpp | 23 +- MIDAS/src/gnc/{ => old_filters}/yessir.h | 4 +- MIDAS/src/gnc/rotation.h | 128 +++- MIDAS/src/sensor_data.h | 2 +- MIDAS/src/systems.cpp | 438 ++++++++------ 14 files changed, 1151 insertions(+), 613 deletions(-) create mode 100644 MIDAS/src/gnc/mqekf.cpp create mode 100644 MIDAS/src/gnc/mqekf.h create mode 100644 MIDAS/src/gnc/mqekf_test.cpp rename MIDAS/src/gnc/{ => old_filters}/example_kf.cpp (93%) rename MIDAS/src/gnc/{ => old_filters}/example_kf.h (92%) rename MIDAS/src/gnc/{ => old_filters}/yessir.cpp (93%) rename MIDAS/src/gnc/{ => old_filters}/yessir.h (87%) diff --git a/MIDAS/src/gnc/constants.h b/MIDAS/src/gnc/constants.h index 57295e30..76525104 100644 --- a/MIDAS/src/gnc/constants.h +++ b/MIDAS/src/gnc/constants.h @@ -5,6 +5,11 @@ const float rho = 1.225; // average air density const float r = 0.0396; // (m) const float height_full = 3.0259; // (m) height of rocket Full Stage const float height_sustainer = 1.5021; // (m) height of rocket Sustainer -const float mass_full = 10.6; // (kg) Sustainer + Booster -const float mass_sustainer = 4.68; // (kg) Sustainer -const float gravity_ms2 = 9.81; // (m/s^2) accel due to gravity \ No newline at end of file +const float mass_sustainer_dry = 3.61; // (kg) Sustainer Dry Mass +const float mass_sustainer_wet = 4.68; // (kg) Sustainer Wet Mass +const float mass_booster_dry = 3.76; // (kg) Booster dry mass +const float mass_booster_wet = 5.91; // (kg) Booster wet mass +const float mass_full = mass_sustainer_wet+mass_booster_wet; // (kg) Total mass wet +const float mass_first_burnout = mass_booster_dry+mass_sustainer_wet;// (kg) Total mass after first stage burnout +const float mass_second_burnout = mass_booster_dry+mass_sustainer_dry;// (kg) Total mass after first stage burnout +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 83183324..64fe39ea 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -1,21 +1,12 @@ #include "ekf.h" -#include "finite-state-machines/fsm_states.h" -#include "rocket_state.h" +#include "fsm_states.h" +#include -extern const std::map O5500X_data; -extern const std::map M685W_data; -extern const std::map> motor_data; EKF::EKF() : KalmanFilter() { - state = KalmanData(); // does this need to change? + 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 @@ -27,8 +18,6 @@ void EKF::priori() {}; * estimate. This process is significantly faster than allowing the state as * letting the filter to converge to the correct state can take up to 3 min. * This specific process was used because the barometric altitude will - * letting the filter to converge to the correct state can take up to 3 min. - * This specific process was used because the barometric altitude will * change depending on the weather and thus, the initial state estimate * cannot be hard coded. A GPS altitude may be used instead but due to GPS * losses during high speed/high altitude flight, it is inadvisable with the @@ -38,93 +27,41 @@ void EKF::priori() {}; */ void EKF::initialize(RocketSystems *args) { - AngularKalmanData angular_kalman = args->rocket_data.angular_kalman_data.getRecentUnsync(); + Orientation orientation = args->rocket_data.orientation.getRecentUnsync(); float sum = 0; for (int i = 0; i < 30; i++) { Barometer barometer = args->rocket_data.barometer.getRecent(); - IMU initial_accelerometer = args->rocket_data.imu.getRecent(); //lowg - Acceleration accelerations = { - .ax = initial_accelerometer.lowg_acceleration.ax, - .ay = initial_accelerometer.lowg_acceleration.ay, - .az = initial_accelerometer.lowg_acceleration.az}; //please check axis divij sum += barometer.altitude; - - // init_accel(0, 0) += -accelerations.ax; - // init_accel(1, 0) += accelerations.ay; - // init_accel(2, 0) += accelerations.az; - THREAD_SLEEP(100); } - // init_accel(0, 0) /= 30; - // init_accel(1, 0) /= 30; - // init_accel(2, 0) /= 30; - - euler_t euler = angular_kalman.getEuler(); - // euler.yaw = -euler.yaw; - - // set x_k + // set x_k - 6 states: [x, vx, y, vy, z, vz] x_k.setZero(); - x_k(0, 0) = sum / 30; - x_k(3, 0) = 0; - x_k(6, 0) = 0; + x_k(0, 0) = sum / 30; // initial altitude (x position) + x_k(2, 0) = 0; // y position + x_k(4, 0) = 0; // z position F_mat.setZero(); // Initialize with zeros + B_mat.setZero(); // Initialize control input matrix - // 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; - Q(1, 1) = pow(s_dt, 3) / 3; // fxed - Q(1, 2) = pow(s_dt, 2) / 2; - Q(2, 2) = s_dt; - Q(1, 0) = Q(0, 1); - Q(2, 0) = Q(0, 2); - Q(2, 1) = Q(1, 2); - - Q(3, 3) = pow(s_dt, 5) / 20; - Q(3, 4) = pow(s_dt, 4) / 8; - Q(3, 5) = pow(s_dt, 3) / 6; - Q(4, 4) = pow(s_dt, 3) / 3; // fixed - Q(4, 5) = pow(s_dt, 2) / 2; - Q(5, 5) = s_dt; - Q(4, 3) = Q(3, 4); - Q(5, 3) = Q(3, 5); - Q(5, 4) = Q(4, 5); - - Q(6, 6) = pow(s_dt, 5) / 20; - Q(6, 7) = pow(s_dt, 4) / 8; - Q(6, 8) = pow(s_dt, 3) / 6; - Q(7, 7) = pow(s_dt, 3) / 3; // fixed - Q(7, 8) = pow(s_dt, 2) / 2; - Q(8, 8) = s_dt; - Q(7, 6) = Q(6, 7); - Q(8, 6) = Q(6, 8); - Q(8, 7) = Q(7, 8); - - // set H + setQ(s_dt, spectral_density_); H.setZero(); - H(0, 0) = 1; - H(1, 2) = 1; - H(2, 5) = 1; - H(3, 8) = 1; - - Q = Q * spectral_density_; + H(0, 0) = 1; // barometer measures x position (altitude) + H(1, 0) = 1; // GPS measures x position (altitude) + H(2, 2) = 1; // GPS measures y position (east) + H(3, 4) = 1; // GPS measures z position (north) P_k.setZero(); - P_k.block<3, 3>(0, 0) = Eigen::Matrix3f::Identity() * 1e-2f; // x block (pos,vel,acc) - P_k.block<3, 3>(3, 3) = Eigen::Matrix3f::Identity() * 1e-2f; // y block - P_k.block<3, 3>(6, 6) = Eigen::Matrix3f::Identity() * 1e-2f; // z block - - // set - R(0, 0) = 10.0; - R(1, 1) = 1.9; - R(2, 2) = 1.9; - R(3, 3) = 1.9; - - // set B (don't care about what's in B since we have no control input) - B(2, 0) = -1; + P_k.block<2, 2>(0, 0) = Eigen::Matrix2f::Identity() * 1e-2f; // x block (pos,vel) + P_k.block<2, 2>(2, 2) = Eigen::Matrix2f::Identity() * 1e-2f; // y block (pos,vel) + P_k.block<2, 2>(4, 4) = Eigen::Matrix2f::Identity() * 1e-2f; // z block (pos,vel) + + // set Measurement Noise Matrix + R(0, 0) = 1.9; // barometer noise + R(1, 1) = 4.0f; // GPS altitude noise (lower trust) + R(2, 2) = 3.0f; // GPS east noise + R(3, 3) = 3.0f; // GPS north noise } /** @@ -135,125 +72,37 @@ void EKF::initialize(RocketSystems *args) * it extrapolates the state at time n+1 based on the state at time n. */ -void EKF::priori(float dt, IMU &imudata, FSMState fsm, AngularKalmanData &angularkalman) +void EKF::priori(float dt, Orientation &orientation, FSMState fsm, Acceleration acceleration) { - Eigen::Matrix xdot = Eigen::Matrix::Zero(); - - // angular states from sensors - Velocity omega_rps = imudata.angular_velocity; // rads per sec - - euler_t angles_rad = angularkalman.getEuler(); - - // ignore effects of gravity when on pad - Eigen::Matrix g_global = Eigen::Matrix::Zero(); - if ((fsm > FSMState::STATE_IDLE)) - { - g_global(0, 0) = -gravity_ms2; - } - else - { - g_global(0, 0) = 0; - } - - // mass and height init - float curr_mass_kg = mass_sustainer; - float curr_height_m = height_sustainer; - - if (fsm < FSMState::STATE_BURNOUT) - { - curr_mass_kg = mass_full; - curr_height_m = height_full; - } - - // Mach number // Subtracting wind from velocity - // float vel_mag_squared_ms = ((x_k(1, 0) - 1.2 * Wind(0, 0)) * (x_k(1, 0) - 1.2 * Wind(0, 0))) + x_k(4, 0) * x_k(4, 0) + x_k(7, 0) * x_k(7, 0); - 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; - - // 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; - - // aerodynamic force - // Body frame - float Fax = 0; // instead of mag square --> mag * vel_x - if ((fsm > FSMState::STATE_IDLE)) - { - Fax = -0.5 * rho * (vel_magnitude_ms) * float(Ca) * (pi * r * r) * x_k(1, 0); - } - float Fay = 0; // assuming no aerodynamic effects - float Faz = 0; // assuming no aerodynamic effects - - // acceleration due to gravity - float gx = g_global(0, 0); - float gy = g_global(1, 0); - float gz = g_global(2, 0); - - // thurst force, body frame - Eigen::Matrix Ft_body; - EKF::getThrust(stage_timestamp, angles_rad, fsm, Ft_body); - - // body frame - float Ftx = Ft_body(0, 0); - float Fty = Ft_body(1, 0); - float Ftz = Ft_body(2, 0); - - Eigen::Matrix velocities_body; - velocities_body << x_k(1, 0), x_k(4, 0), x_k(7, 0); - - GlobalToBody(angles_rad, velocities_body); - float vx_body = velocities_body(0, 0); - float vy_body = velocities_body(1, 0); - float vz_body = velocities_body(2, 0); - - Eigen::Matrix v_dot; // we compute everything in the body frame for accelerations, and then convert those accelerations to global frame - v_dot << ((Fax + Ftx) / curr_mass_kg - (omega_rps.vy * vz_body - omega_rps.vz * vy_body) + x_k(2, 0)) / 2, - ((Fay + Fty) / curr_mass_kg - (omega_rps.vz * vx_body - omega_rps.vx * vz_body) + x_k(5, 0) / 2), - ((Faz + Ftz) / curr_mass_kg - (omega_rps.vx * vy_body - omega_rps.vy * vx_body) + x_k(8, 0) / 2); - - BodyToGlobal(angles_rad, v_dot); - - xdot << x_k(1, 0), v_dot(0, 0) + gx, - 0.0, - - x_k(4, 0), v_dot(1, 0) + gy, - 0.0, - - x_k(7, 0), v_dot(2, 0) + gz, - 0.0; - - // priori step - x_priori = (xdot * dt) + x_k; - - float coeff = 0; - if ((fsm > FSMState::STATE_IDLE)) - { - coeff = -pi * Ca * (r * r) * rho / curr_mass_kg; - } - - setF(dt, omega_rps.vx, omega_rps.vy, omega_rps.vz, coeff, vx_body, vy_body, vz_body); - + setF(dt); + setB(dt); + // Compute control input at current time step + Eigen::Matrix sensor_accel_global_g = Eigen::Matrix::Zero(); + sensor_accel_global_g(0, 0) = acceleration.ax + 0.045f; + sensor_accel_global_g(1, 0) = acceleration.ay - 0.065f; + sensor_accel_global_g(2, 0) = acceleration.az - 0.06f; + // euler_t angles_rad = orientation.getEuler(); + // BodyToGlobal(angles_rad, sensor_accel_global_g); + // Do not apply gravity if on pad + float g_ms2 = (fsm > FSMState::STATE_IDLE) ? gravity_ms2 : 0.0f; + u_control(0, 0) = sensor_accel_global_g(0, 0) * g_ms2; + u_control(1, 0) = sensor_accel_global_g(1, 0) * g_ms2; + u_control(2, 0) = sensor_accel_global_g(2, 0) * g_ms2; + // Predict state at time t and covariance + x_priori = F_mat * x_k + B_mat * u_control; P_priori = (F_mat * P_k * F_mat.transpose()) + Q; } - /** - * @brief Update Kalman Gain and state estimate with current sensor data + * @brief Update state estimate with current sensor data * - * After receiving new sensor data, the Kalman filter updates the state estimate - * and Kalman gain. The Kalman gain can be considered as a measure of how uncertain - * the new sensor data is. After updating the gain, the state estimate is updated. + * After receiving new sensor data, the Kalman filter updates the state estimate. + * Updates with barometer (always) and GPS (if available). * */ -void EKF::update(Barometer barometer, Acceleration acceleration, AngularKalmanData angularkalman, FSMState FSM_state) +void EKF::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state, GPS &gps) { - // if on pad -> take last 10 barometer measurements for init state + // if on pad take last 10 barometer measurements for init state if (FSM_state == FSMState::STATE_IDLE) { float sum = 0; @@ -263,71 +112,116 @@ void EKF::update(Barometer barometer, Acceleration acceleration, AngularKalmanDa { sum += i; } - P_k(4, 4) = 1e-6f; // variance for vel_y - P_k(7, 7) = 1e-6f; // variance for vel_z 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 sensor_accel_global_g = Eigen::Matrix(Eigen::Matrix::Zero()); - // accouting for sensor bias and coordinate frame transforms - (sensor_accel_global_g)(0, 0) = -acceleration.ax + 0.045; - (sensor_accel_global_g)(1, 0) = acceleration.ay - 0.065; - (sensor_accel_global_g)(2, 0) = acceleration.az - 0.06; + // Update GPS reference point if in IDLE state (needed before GPS measurement update) + reference_GPS(gps, FSM_state); - euler_t angles_rad = angularkalman.getEuler(); - // angles_rad.yaw = -angles_rad.yaw; // coordinate frame match - - BodyToGlobal(angles_rad, sensor_accel_global_g); - - float g_ms2; - if ((FSM_state > FSMState::STATE_IDLE)) + // Check if GPS has valid fix + bool gps_available = (gps.fix_type != 0 && gps.latitude != 0 && gps.longitude != 0); + + // Build measurement vector and select appropriate H and R matrices + Eigen::Matrix identity = Eigen::Matrix::Identity(); + + if (gps_available) { - g_ms2 = gravity_ms2; + // GPS available: use full H matrix + float lat = gps.latitude / 1e7; // dividing by 1e7 to convert from int to float + float lon = gps.longitude / 1e7; + float alt = gps.altitude; + // Convert GPS to ECEF + std::vector rocket_cords = gps_to_ecef(lat, lon, alt); + std::vector reference_cord = gps_to_ecef(gps_latitude_original, gps_longitude_original, 0); + float gps_latitude_original_rad = gps_latitude_original * pi / 180; + float gps_longitude_original_rad = gps_longitude_original * pi / 180; + double dx = rocket_cords[0] - reference_cord[0]; + double dy = rocket_cords[1] - reference_cord[1]; + double dz = rocket_cords[2] - reference_cord[2]; + float east = -std::sin(gps_longitude_original_rad) * dx + std::cos(gps_longitude_original_rad) * dy; + float north = -std::sin(gps_latitude_original_rad) * std::cos(gps_longitude_original_rad) * dx - std::sin(gps_latitude_original_rad) * std::sin(gps_longitude_original_rad) * dy + std::cos(gps_latitude_original_rad) * dz; + // Build measurement vector + Eigen::Matrix y_combined; + y_combined(0) = barometer.altitude; // barometer altitude + y_combined(1) = alt; // GPS altitude + y_combined(2) = east; // GPS east + y_combined(3) = north; // GPS north + // Innovation + Eigen::Matrix innovation = y_combined - H * x_priori; + // Innovation covariance + Eigen::Matrix S = H * P_priori * H.transpose() + R; + // Kalman gain + Eigen::Matrix K = P_priori * H.transpose() * S.inverse(); + // Update state and covariance + x_k = x_priori + K * innovation; + P_k = (identity - K * H) * P_priori; + + gps_latitude_last = lat; + gps_longitude_last = lon; } else { - g_ms2 = 0; + // GPS not available, use only barometer H matrix + Eigen::Matrix H_baro = H.block<1, NUM_STATES>(0, 0); + Eigen::Matrix R_baro; + R_baro(0, 0) = R(0, 0); + // Build measurement vector: [baro_alt] + Eigen::Matrix y_baro; + y_baro(0) = barometer.altitude; + + // Innovation + Eigen::Matrix innovation = y_baro - H_baro * x_priori; + // Innovation covariance + Eigen::Matrix S = H_baro * P_priori * H_baro.transpose() + R_baro; + // Kalman gain + Eigen::Matrix K = P_priori * H_baro.transpose() / S(0, 0); + // Update state and covariance + x_k = x_priori + K * innovation; + P_k = (identity - K * H_baro) * P_priori; } - // acceloremeter reports values in g's and measures specific force - y_k(1, 0) = ((sensor_accel_global_g)(0)) * g_ms2; - y_k(2, 0) = ((sensor_accel_global_g)(1)) * g_ms2; - y_k(3, 0) = ((sensor_accel_global_g)(2)) * g_ms2; - - y_k(0, 0) = barometer.altitude; // meters - - // # Posteriori Update - x_k = x_priori + K * (y_k - (H * x_priori)); - P_k = (identity - K * H) * P_priori; + bool is_landed = (FSM_state == FSMState::STATE_LANDED); + if (is_landed) + { + if (was_landed_last) + { + landed_state_duration += s_dt; // Accumulate time in LANDED state + } + else + { + landed_state_duration = s_dt; // Reset timer + } + + // Only reset velocities if we've been landed for at least 0.5 seconds + if (landed_state_duration >= 0.5f) + { + x_k(3, 0) = 0.0f; // velocity y (vy) = 0 when landed + x_k(5, 0) = 0.0f; // velocity z (vz) = 0 when landed + } + } + else + { + // Negate velocity z if it's negative (so negative becomes positive) + if (x_k(5, 0) < 0) // velocity z (vz) + { + x_k(5, 0) = -x_k(5, 0); // Negate negative velocity z to make it positive + } + } + // Update output state structure kalman_state.state_est_pos_x = x_k(0, 0); kalman_state.state_est_vel_x = x_k(1, 0); - kalman_state.state_est_accel_x = x_k(2, 0); - kalman_state.state_est_pos_y = x_k(3, 0); - kalman_state.state_est_vel_y = x_k(4, 0); - kalman_state.state_est_accel_y = x_k(5, 0); - kalman_state.state_est_pos_z = x_k(6, 0); - kalman_state.state_est_vel_z = x_k(7, 0); - kalman_state.state_est_accel_z = x_k(8, 0); - + kalman_state.state_est_accel_x = u_control(0, 0); + kalman_state.state_est_pos_y = x_k(2, 0); + kalman_state.state_est_vel_y = x_k(3, 0); + kalman_state.state_est_accel_y = u_control(1, 0); + kalman_state.state_est_pos_z = x_k(4, 0); + kalman_state.state_est_vel_z = x_k(5, 0); + kalman_state.state_est_accel_z = u_control(2, 0); state.position = (Position){kalman_state.state_est_pos_x, kalman_state.state_est_pos_y, kalman_state.state_est_pos_z}; state.velocity = (Velocity){kalman_state.state_est_vel_x, kalman_state.state_est_vel_y, kalman_state.state_est_vel_z}; state.acceleration = (Acceleration){kalman_state.state_est_accel_x, kalman_state.state_est_accel_y, kalman_state.state_est_accel_z}; - } /** @@ -337,23 +231,31 @@ void EKF::update(Barometer barometer, Acceleration acceleration, AngularKalmanDa * @param sd Spectral density of the noise * @param &barometer Data of the current barometer * @param acceleration Current acceleration - * @param &orientation Current orientation + * @param &orientation Current orientation * @param current_state Current FSM_state */ -void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, IMU &imudata ,AngularKalmanData &angularkalman, FSMState FSM_state) +void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState FSM_state, GPS &gps) { + if (FSM_state >= FSMState::STATE_IDLE) // { if (FSM_state != last_fsm) { stage_timestamp = 0; last_fsm = FSM_state; + // Reset landed state tracking when FSM changes + if (FSM_state != FSMState::STATE_LANDED) + { + landed_state_duration = 0.0f; + was_landed_last = false; + } } stage_timestamp += dt; - // setF(dt, orientation.roll, orientation.pitch, orientation.yaw); + s_dt = dt; // Store dt for use in update + setQ(dt, sd); - priori(dt, imudata, FSM_state, angularkalman); - update(barometer, acceleration, angularkalman, FSM_state); + priori(dt, orientation, FSM_state, acceleration); + update(barometer, acceleration, orientation, FSM_state, gps); } } @@ -394,39 +296,26 @@ void EKF::setState(KalmanState state) * The Q matrix is the covariance matrix for the process noise and is * updated based on the time taken per cycle of the Kalman Filter Thread. */ -void EKF:: - setQ(float dt, float sd) +void EKF::setQ(float dt, float sd) { - Q(0, 0) = pow(dt, 5) / 20; - Q(0, 1) = pow(dt, 4) / 8; - Q(0, 2) = pow(dt, 3) / 6; - Q(1, 1) = pow(dt, 3) / 3; - Q(1, 2) = pow(dt, 2) / 2; - Q(2, 2) = dt; - Q(1, 0) = Q(0, 1); - Q(2, 0) = Q(0, 2); - Q(2, 1) = Q(1, 2); - Q(3, 3) = pow(dt, 5) / 20; - Q(3, 4) = pow(dt, 4) / 8; - Q(3, 5) = pow(dt, 3) / 6; - Q(4, 4) = pow(dt, 3) / 3; - Q(4, 5) = pow(dt, 2) / 2; - Q(5, 5) = dt; - Q(4, 3) = Q(3, 4); - Q(5, 3) = Q(3, 5); - Q(5, 4) = Q(4, 5); - - Q(6, 6) = pow(dt, 5) / 20; - Q(6, 7) = pow(dt, 4) / 8; - Q(6, 8) = pow(dt, 3) / 6; - Q(7, 7) = pow(dt, 3) / 3; - Q(7, 8) = pow(dt, 2) / 2; - Q(8, 8) = dt; - Q(7, 6) = Q(6, 7); - Q(8, 6) = Q(6, 8); - Q(8, 7) = Q(7, 8); - - Q *= sd; + // discrete-time integrated acceleration noise + float sigma_a = 0.2f; + Q.setZero(); + // X axis + Q(0,0) = pow(dt,4)/4.0f * sigma_a*sigma_a; + Q(0,1) = pow(dt,3)/2.0f * sigma_a*sigma_a; + Q(1,0) = Q(0,1); + Q(1,1) = pow(dt,2) * sigma_a*sigma_a; + // Y axis + Q(2,2) = pow(dt,4)/4.0f * sigma_a*sigma_a; + Q(2,3) = pow(dt,3)/2.0f * sigma_a*sigma_a; + Q(3,2) = Q(2,3); + Q(3,3) = pow(dt,2) * sigma_a*sigma_a; + // Z axis + Q(4,4) = pow(dt,4)/4.0f * sigma_a*sigma_a; + Q(4,5) = pow(dt,3)/2.0f * sigma_a*sigma_a; + Q(5,4) = Q(4,5); + Q(5,5) = pow(dt,2) * sigma_a*sigma_a; } /** @@ -438,103 +327,39 @@ void EKF:: * by how the states change over time and also depends on the * current state of the rocket. */ -void EKF::setF(float dt, float w_x, float w_y, float w_z, float coeff, float v_x, float v_y, float v_z) - +void EKF::setF(float dt) { - F_mat.setIdentity(); // start from identity - - // For x - // F_mat(0, 1) = dt; - // F_mat(0, 2) = 0.5f * dt * dt; - // F_mat(1, 2) = dt; - - // // For y - // F_mat(3, 4) = dt; - // F_mat(3, 5) = 0.5f * dt * dt; - // F_mat(4, 5) = dt; - - // // For z - // F_mat(6, 7) = dt; - // F_mat(6, 8) = 0.5f * dt * dt; - // F_mat(7, 8) = dt; - F_mat.setZero(); - - F_mat(0, 1) = 1.0f; - - F_mat(1, 1) = coeff * v_x; - F_mat(1, 2) = 0.5f; - F_mat(1, 4) = 0.5f * (coeff * v_y + w_z); - F_mat(1, 7) = 0.5f * (coeff * v_z - w_y); - - F_mat(3, 4) = 1.0f; - - F_mat(4, 1) = -0.5f * w_z; - F_mat(4, 5) = 0.5f; - F_mat(4, 7) = 0.5f * w_x; - - F_mat(6, 7) = 1.0f; - - F_mat(7, 1) = 0.5f * w_y; - F_mat(7, 4) = -0.5f * w_x; - F_mat(7, 8) = 0.5f; + F_mat.setIdentity(); + F_mat(0, 1) = dt; // x += vx * dt + F_mat(2, 3) = dt; // y += vy * dt + F_mat(4, 5) = dt; // z += vz * dt } -/** - * @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) +void EKF::setB(float dt) { - // Pick which motor thrust curve to use - const std::map *thrust_curve = nullptr; + B_mat.setZero(); + B_mat(0, 0) = 0.5f*dt*dt; // x += 1/2 * ax * dt^2 + B_mat(1, 0) = dt; // vx += ax * dt + B_mat(2, 1) = 0.5f*dt*dt; // y += 1/2 * ay * dt^2 + B_mat(3, 1) = dt; // vy += ay * dt + B_mat(4, 2) = 0.5f*dt*dt; // z += 1/2 * az * dt^2 + B_mat(5, 2) = dt; // vz += az * dt +} - if (FSM_state == STATE_FIRST_BOOST){ - thrust_curve = &motor_data.at("Booster"); // Booster - } - else if (FSM_state == STATE_SECOND_BOOST) - thrust_curve = &motor_data.at("Sustainer"); // Sustainer - else +void EKF::reference_GPS(GPS &gps, FSMState fsm) +{ + if (gps.latitude == 0 || gps.longitude == 0) { - thrust_out.setZero(); - return; // No thrust before ignition + return; // No GPS fix skip reference update } - // 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) + if (fsm == FSMState::STATE_IDLE) { - thrust_out.setZero(); // assume motor burned out after curve ends + gps_latitude_original = gps.latitude / 1e7; + gps_longitude_original = gps.longitude / 1e7; + gps_latitude_last = gps_latitude_original; + gps_longitude_last = gps_longitude_original; } - 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; +EKF ekf; \ No newline at end of file diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index fabd94c4..6c59e8da 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -2,16 +2,18 @@ #include "kalman_filter.h" #include "sensor_data.h" -#include "Buffer.h" +#include "Buffer.h" #include "constants.h" #include "aero_coeff.h" #include "rotation.h" -#define NUM_STATES 9 -#define NUM_SENSOR_INPUTS 4 +#define NUM_STATES 6 // [x, vx, y, vy, z, vz] - position and velocity only +#define NUM_SENSOR_INPUTS 4 // barometer, gps_x, gps_y, gps_z +#define NUM_CONTROL_INPUTS 3 // acceleration as control input [ax, ay, az] #define ALTITUDE_BUFFER_SIZE 10 -// Number of entries for aerodynamic data table + +// Number of entries for aerodynamic data table #define AERO_DATA_SIZE (sizeof(aero_data) / sizeof(aero_data[0])) class EKF : public KalmanFilter @@ -19,22 +21,19 @@ class EKF : public KalmanFilter public: EKF(); void initialize(RocketSystems* args) override; - void priori(); - void priori(float dt, IMU &imudata, FSMState fsm, AngularKalmanData &angularkalman); - void update(Barometer barometer, Acceleration acceleration, AngularKalmanData angularkalman, FSMState FSM_state) override; + // void priori(); + void priori(float dt, Orientation &orientation, FSMState fsm, Acceleration acceleration); + void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState state, GPS &gps) override; void setQ(float dt, float sd); - void setF(float dt, float w_x, float w_y, float w_z, float coeff, float v_x,float v_y, float v_z); - - // void BodyToGlobal(euler_t angles, Eigen::Matrix &body_vec); - // void GlobalToBody(euler_t angles, Eigen::Matrix &global_vec); + void setF(float dt); + void setB(float dt); // Set control input matrix for acceleration KalmanData getState() override; void setState(KalmanState state) override; + void reference_GPS(GPS &gps, FSMState fsm); - 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, IMU &imudata ,AngularKalmanData &angularkalman, FSMState FSM_state); + void tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState state, GPS &gps); bool should_reinit = false; float current_vel = 0.0f; @@ -42,21 +41,40 @@ class EKF : public KalmanFilter private: float s_dt = 0.05f; - float spectral_density_ = 0.001f; + float spectral_density_ = 13.0f; float kalman_apo = 0; float Ca = 0; float Cn = 0; float Wind_alpha = 0.85f; float Cp = 0; - + float curr_mass_kg = mass_full; //(kg) Sustainer + Booster, but value changes over time. + std::vector starting_gps; // latitude, longitude, altitude + std::vector starting_ecef; // x, y, z + // Eigen::Matrix gravity = Eigen::Matrix::Zero(); KalmanState kalman_state; FSMState last_fsm = FSMState::STATE_IDLE; float stage_timestamp = 0; + + // Track how long we've been in LANDED state to avoid false positives + float landed_state_duration = 0.0f; + bool was_landed_last = false; Eigen::Matrix init_accel = Eigen::Matrix::Zero(); Buffer alt_buffer; KalmanData state; + + // Control input matrix for acceleration [ax, ay, az] + Eigen::Matrix B_mat; + + // Last computed control input (acceleration) - filled by priori(), used by update() for state output + Eigen::Matrix u_control; + + // GPS reference coordinates + float gps_latitude_original = 0.0f; + float gps_longitude_original = 0.0f; + float gps_latitude_last = 0.0f; + float gps_longitude_last = 0.0f; }; diff --git a/MIDAS/src/gnc/kalman_filter.h b/MIDAS/src/gnc/kalman_filter.h index 67f9301f..113db0aa 100644 --- a/MIDAS/src/gnc/kalman_filter.h +++ b/MIDAS/src/gnc/kalman_filter.h @@ -6,8 +6,9 @@ #undef B1 #include -#include "sensor_data.h" -#include "systems.h" +// #include "systems.h" +#include "sensor_data.h" // For sim +#include "systems.h" // for sim struct KalmanState { @@ -57,8 +58,10 @@ class KalmanFilter } virtual void initialize(RocketSystems* args) = 0; - virtual void priori() = 0; - virtual void update(Barometer barometer, Acceleration acceleration, AngularKalmanData angularkalman, FSMState FSM_state) = 0; + // virtual void priori() = 0; + virtual void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState current_state,GPS &gps) = 0; + virtual void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState current_state,GPS &gps) = 0; virtual KalmanData getState() = 0; virtual void setState(KalmanState state) = 0; + }; diff --git a/MIDAS/src/gnc/mqekf.cpp b/MIDAS/src/gnc/mqekf.cpp new file mode 100644 index 00000000..18855d69 --- /dev/null +++ b/MIDAS/src/gnc/mqekf.cpp @@ -0,0 +1,344 @@ +#include "mqekf.h" +#include "fsm_states.h" + +/* + mag + y -> -x + x -> y + z -> -z + + acc + pitch -> roll + roll -> pitch +*/ + +void QuaternionMEKF::initialize(RocketSystems *args) +{ + float Pq0 = 1e-6; + float Pb0 = 1e-1; + Q = initialize_Q(sigma_g); + + Eigen::Matrix sigmas; + sigmas << sigma_a, sigma_m; + + sigma_a = {160 * sqrt(100.0f) * 1e-6 * 9.81, 160 * sqrt(100.0f) * 1e-6 * 9.81, 190 * sqrt(100.0f) * 1e-6 * 9.81}; // ug/sqrt(Hz) *sqrt(hz). values are from datasheet + sigma_g = {0.1 * pi / 180, 0.1 * pi / 180, 0.1 * pi / 180}; // 0.1 deg/s + sigma_m = {0.4e-4 / sqrt(3), 0.4e-4 / sqrt(3), 0.4e-4 / sqrt(3)}; // 0.4 mG -> T, it is 0.4 total so we divide by sqrt3 + + R = sigmas.array().square().matrix().asDiagonal(); + + qref.setIdentity(); // 1,0,0,0 + x.setZero(); + P.setZero(); + P.block<3, 3>(0, 0) = Pq0 * Eigen::Matrix3f::Identity(); + P.block<3, 3>(3, 3) = Pb0 * Eigen::Matrix3f::Identity(); + + Acceleration accel, accel_sum; + Magnetometer mag, mag_sum; + FSMState FSM_state = arg->rocket_data.fsm_state.getRecent(); + if (FSM_state == FSMState::STATE_IDLE) + { + for (int i = 0; i < 10; i++) + { + accel = args->rocket_data.imu.highg_acceleration.getRecent(); + accel_sum += accel; + mag = args->rocket_data.magnetometer.getRecent(); + mag_sum += mag; + } + accel = accel_sum /= 10; + else + { + accel = args->rocket_data.imu.highg_acceleration.getRecent(); + mag = args->rocket_data.magnetometer.getRecent(); + } + + // MagnetometerSensor mag = mag_sum / 10; // args->rocket_data.magnetometer.getRecentUnsync(); + // Acceleration accel = accel_sum / 10; // args->rocket_data.imu.Velocity.getRecent(); + initialize_from_acc_mag(accel, mag); + } +} + +QuaternionMEKF::QuaternionMEKF() +{ + state = AngularKalmanData(); +} + +void QuaternionMEKF::tick(float dt, Magnetometer &magnetometer, Velocity &angular_velocity, Acceleration &acceleration, FSMState FSM_state) +{ + if (FSM_state >= FSMState::STATE_IDLE) // + { + if (FSM_state != last_fsm) + { + stage_timestamp = 0; + last_fsm = FSM_state; + // Reset landed state tracking when FSM changes + if (FSM_state != FSMState::STATE_LANDED) + { + landed_state_duration = 0.0f; + was_landed_last = false; + } + } + stage_timestamp += dt; + s_dt = dt; // Store dt for use in update + + // setQ(dt, sd); + // priori(dt, orientation, FSM_state, acceleration); + // update(barometer, acceleration, orientation, FSM_state, gps); + + time_update(angular_velocity, dt) + measurement_update(acceleration, magnetometer); + Eigen curr_quat = quat(); // w,x,y,z + + state.quat.w = curr_quat(0, 0); + state.quat.x = curr_quat(1, 0); + state.quat.y = curr_quat(2, 0); + state.quat.z = curr_quat(3, 0); + state.has_data = true; // not sure what this is + + Eigen::Matrix orientation = quatToEuler(quat); + state.roll = orientation(0, 0); + state.pitch = orientation(1, 0); + state.yaw = orientation(2, 0); + + Eigen::Matrix bias_gyro = gyroscope_bias(); + state.gyrobias[0] = bias_gyro(0, 0); + state.gyrobias[1] = bias_gyro(1, 0); + state.gyrobias[2] = bias_gyro(2, 0); + } +} + +void QuaternionMEKF::time_update(Velocity const &gyro, float Ts) +{ + // Conversion from degrees/s to radians/s + + Eigen::Matrix gyr; + gyr(0, 0) = gyro.vx * (pi / 180.0f); + gyr(1, 0) = gyro.vy * (pi / 180.0f); + gyr(2, 0) = gyro.vz * (pi / 180.0f); + + set_transition_matrix(gyr - x.tail(3), Ts); + + Eigen::Vector4f q; // necessary to reorder to w,x,y,z + q << qref.w(), qref.x(), qref.y(), qref.z(); + + q = F * q; + + qref = Eigen::Quaternionf(q(0), q(1), q(2), q(3)); + qref.normalize(); + + Eigen::Matrix F_a; + // Slice 3x3 block from F + F_a << F.block(0, 0, 3, 3), (-Eigen::Matrix::Identity() * Ts), + Eigen::Matrix::Zero(), Eigen::Matrix::Identity(); + P = F_a * P * F_a.transpose() + Q; // P update +} + +void QuaternionMEKF::measurement_update(Acceleration const &accel, Magnetometer const &magn) +{ + // accel measurements + Eigen::Matrix acc; + acc(0, 0) = accel.ax; + acc(1, 0) = accel.ay; + acc(2, 0) = accel.az; + + Eigen::Matrix mag; + mag(0, 0) = magn.mx; + mag(1, 0) = magn.my; + mag(2, 0) = magm.mz; + + Eigen::Matrix const v1hat = accelerometer_measurement_func(); + Eigen::Matrix const v2hat = magnetometer_measurement_func(); + + Eigen::Matrix const C1 = skew_symmetric_matrix(v1hat); + Eigen::Matrix const C2 = skew_symmetric_matrix(v2hat); + + Eigen::Matrix C; + C << C1, Eigen::Matrix::Zero(), + C2, Eigen::Matrix::Zero(); + + Eigen::Matrix yhat; + yhat << v1hat, + v2hat; + + Eigen::Matrix y; + y << acc, + mag; + + Eigen::Matrix inno = y - yhat; + + Eigen::Matrix const s = C * P * C.transpose() + R; + + // K = P * C.float *(s)^-1 + // K * s = P*C.float + + // This is the form + // x * A = b + // Which can be solved with the code below + Eigen::FullPivLU> lu(s); // LU decomposition of s + if (lu.isInvertible()) + { + Eigen::Matrix const K = P * C.transpose() * lu.inverse(); // gain + + x += K * inno; // applying correction??? + + // Joseph form of covariance measurement update + Eigen::Matrix const temp = Eigen::Matrix::Identity() - K * C; + P = temp * P * temp.transpose() + K * R * K.transpose(); // covariance update??? + // Apply correction to qref + Eigen::Quaternion corr(1, 0.5f * x(0), 0.5f * x(1), 0.5f * x(2)); // small angle approx???? + corr.normalize(); + qref = qref * corr; // multiply quaternions from ref??????? + + // We only want to reset the quaternion part of the state + x(0) = 0; + x(1) = 0; + x(2) = 0; + } +} + +void QuaternionMEKF::measurement_update_partial( + Eigen::Matrix const &meas, + Eigen::Ref const> const &vhat, + Eigen::Ref const> const &Rm) +{ + // Predicted measurement Jacobian + Eigen::Matrix const C1 = skew_symmetric_matrix(vhat); + + Eigen::Matrix C; + + C << C1, Eigen::Matrix::Zero(); + + // Innovation + Eigen::Matrix const inno = meas - vhat; + + // Innovation covariance + Eigen::Matrix const s = C * P * C.transpose() + Rm; + + // K = P * C.T * s^-1 + Eigen::FullPivLU> lu(s); + if (lu.isInvertible()) + { + Eigen::Matrix const K = P * C.transpose() * lu.inverse(); + + // State update + x += K * inno; + + // Joseph form covariance update + Eigen::Matrix const temp = + Eigen::Matrix::Identity() - K * C; + P = temp * P * temp.transpose() + K * Rm * K.transpose(); + + // Apply correction to reference quaternion (small-angle approx) + Eigen::Quaternion corr( + 1.0f, + 0.5f * x(0), + 0.5f * x(1), + 0.5f * x(2)); + corr.normalize(); + qref = qref * corr; + + // Reset attitude error states + x(0) = 0.0f; + x(1) = 0.0f; + x(2) = 0.0f; + } +} + +Eigen::Matrix QuaternionMEKF::quaternion() +{ + return qref.coeffs(); +} + +void QuaternionMEKF::set_transition_matrix(Eigen::Ref> const &gyr, float Ts) +{ + Eigen::Matrix const delta_theta = gyr * Ts; + float un = delta_theta.norm(); + if (un == 0) + un = std::numeric_limits::min(); + + Eigen::Matrix const Omega = (Eigen::Matrix() << -skew_symmetric_matrix(delta_theta), delta_theta, + -delta_theta.transpose(), 0) + .finished(); + + F = std::cos(0.5f * un) * Eigen::Matrix::Identity() + std::sin(0.5f * un) / un * Omega; +} + +Eigen::Matrix QuaternionMEKF::skew_symmetric_matrix(const Eigen::Ref> &vec) const +{ + Eigen::Matrix M; + M << 0, -vec(2), vec(1), + vec(2), 0, -vec(0), + -vec(1), vec(0), 0; + + return M; +} + +Eigen::Matrix QuaternionMEKF::accelerometer_measurement_func() const +{ + return qref.inverse() * v1ref; +} + +Eigen::Matrix QuaternionMEKF::magnetometer_measurement_func() const +{ + return qref.inverse() * v2ref; +} + +Eigen::Matrix QuaternionMEKF::initialize_Q(Eigen::Matrix sigma_g) +{ + Eigen::Matrix Q = Eigen::Matrix::Zero(); + Q.block<3, 3>(0, 0) = sigma_g.array().square().matrix().asDiagonal(); + Q.block<3, 3>(3, 3) = 1e-12 * Eigen::Matrix3f::Identity(); + return Q; +} + +void QuaternionMEKF::initialize_from_acc_mag(Eigen::Matrix const &acc, Eigen::Matrix const &mag) +{ + float const anorm = acc.norm(); + v1ref << anorm, 0, 0; + + Eigen::Matrix const acc_normalized = acc / anorm; + Eigen::Matrix const mag_normalized = mag.normalized(); + + Eigen::Matrix const Rz = -acc_normalized; + Eigen::Matrix const Ry = Rz.cross(mag_normalized).normalized(); + Eigen::Matrix const Rx = Ry.cross(Rz).normalized(); + + // Construct the rotation matrix + Eigen::Matrix const R = (Eigen::Matrix() << Rx, Ry, Rz).finished(); + + // Eigen can convert it to a quaternion + qref = R.transpose(); + + // Reference magnetic field vector + v2ref = qref * mag; +} + +Eigen::Matrix QuaternionMEKF::gyroscope_bias() +{ + return x.tail(3); +} + +Eigen::Matrix QuaternionMEKF::quatToEuler(const Eigen::Matrix &q) +{ + + double w = q[0]; + double x = q[1]; + double y = q[2]; + double z = q[3]; + + double sinr_cosp = 2.0 * (w * x + y * z); + double cosr_cosp = 1.0 - 2.0 * (x * x + y * y); + double roll = std::atan2(sinr_cosp, cosr_cosp); + + double sinp = 2.0 * (w * y - z * x); + double pitch; + if (std::abs(sinp) >= 1) + pitch = std::copysign(pi / 2, sinp); + else + pitch = std::asin(sinp); + + double siny_cosp = 2.0 * (w * z + x * y); + double cosy_cosp = 1.0 - 2.0 * (y * y + z * z); + double yaw = std::atan2(siny_cosp, cosy_cosp); + return Eigen::Matrix(roll, pitch, yaw); +} diff --git a/MIDAS/src/gnc/mqekf.h b/MIDAS/src/gnc/mqekf.h new file mode 100644 index 00000000..c4a16afe --- /dev/null +++ b/MIDAS/src/gnc/mqekf.h @@ -0,0 +1,62 @@ +#pragma once + +#include "sensor_data.h" // for sim +#include "Buffer.h" // for sim +#include "constants.h" +#include + +class QuaternionMEKF +{ +public: + QuaternionMEKF(const Eigen::Matrix &sigma_a, + const Eigen::Matrix &sigma_g, + const Eigen::Matrix &sigma_m); + + void intialize(RocketSystems *arg); + void initialize_from_acc_mag(Eigen::Matrix const &acc, Eigen::Matrix const &mag); + void time_update(Velocity const &gyro, float Ts); + void measurement_update(Acceleration const &acc, Magnetometer const &mag); + void measurement_update_partial( + Eigen::Matrix const &meas, + Eigen::Ref const> const &vhat, + Eigen::Ref const> const &Rm); + void tick(float dt, Magnetometer &magnetometer, Velocity &angular_velocity, Acceleration &acceleration, FSMState FSM_state); + Eigen::Matrix quaternion(); + Eigen::Matrix covariance(); + Eigen::Matrix gyroscope_bias(); + Eigen::Matrix accelerometer_measurement_func() const; + Eigen::Matrix Racc, Rmag; + + + Eigen::Matrix quatToEuler(const Eigen::Matrix &q); + // void tick(float dt, float sd, ); + +private: + Eigen::Quaternion qref; + AngularKalmanData state; + + Eigen::Matrix sigma_a; + Eigen::Matrix sigma_g; + Eigen::Matrix sigma_m; + Eigen::Matrix v1ref; + Eigen::Matrix v2ref; + + // State + Eigen::Matrix x; + // State covariance + Eigen::Matrix P; + + // Quaternion update matrix + Eigen::Matrix F; + + Eigen::Matrix R; + Eigen::Matrix Q; + + void set_transition_matrix(const Eigen::Ref> &gyr, float Ts); + Eigen::Matrix skew_symmetric_matrix(const Eigen::Ref> &vec) const; + Eigen::Matrix magnetometer_measurement_func() const; + + static Eigen::Matrix initialize_Q(Eigen::Matrix sigma_g); +}; + +extern QuaternionMEKF mqekf; diff --git a/MIDAS/src/gnc/mqekf_test.cpp b/MIDAS/src/gnc/mqekf_test.cpp new file mode 100644 index 00000000..e4d76097 --- /dev/null +++ b/MIDAS/src/gnc/mqekf_test.cpp @@ -0,0 +1,124 @@ +#include "mqekf.h" +#include +#include +#include +#include +using namespace Eigen; + +int main(int argc, char *argv[]) + +{ + if (argc < 2) + { + std::cerr << "Usage: " << argv[0] << " " << std::endl; + return 1; + } + + std::string input_file = argv[1]; + Eigen::Matrix sigma_a = {160 * sqrt(100.0f) * 1e-6 * 9.81, 160 * sqrt(100.0f) * 1e-6 * 9.81, 190 * sqrt(100.0f) * 1e-6 * 9.81}; // ug/sqrt(Hz) *sqrt(hz). values are from datasheet + Eigen::Matrix sigma_g = {0.1 * M_PI / 180, 0.1 * M_PI / 180, 0.1 * M_PI / 180}; // 0.1 deg/s + Eigen::Matrix sigma_m = {0.4e-4 / sqrt(3), 0.4e-4 / sqrt(3), 0.4e-4 / sqrt(3)}; // 0.4 mG -> T, it is 0.4 total so we divide by sqrt3 + + // QuaternionMEKF mekf(sigma_a, sigma_g, sigma_m); + + // Eigen::Matrix acc0 = {9.81, 0, 0}; // Factored in + // Eigen::Matrix mag0 = {-0.34, -0.01, 0.75}; // Factored in ?? + + // mekf.initialize_from_acc_mag(acc0, mag0); + Eigen::Matrix quat = mekf.quaternion(); + + std::cout << "[" << quat[0] << ", " << quat[1] << ", " << quat[2] << ", " << quat[3] << "]," << std::endl; + + Eigen::Matrix gyr; + Eigen::Matrix acc; + Eigen::Matrix mag; + Eigen::Matrix gps; + Eigen::Matrix orientation; + double alt; + double time; + + std::ifstream file(input_file); + + if (!file.is_open()) + { + std::cerr << "Error opening file!" << std::endl; + return 1; + } + + // Vectors to store the sensor data. Each element is a 3D vector + std::vector> accel_pull; // [ax, ay, az] for each timestamp + std::vector> gyro_pull; // [wx, wy, wz] for each timestamp rad/s + std::vector> mag_pull; // [mx, my, mz] for each timestamp + std::vector> gps_pull; // [lat,long] for each timestamp + std::vector> altitude_pull; // [alt] for each timestamp + std::vector> time_pull; // [time] for each timestamp + + std::string line; + std::getline(file, line); + + while (std::getline(file, line)) + { + std::stringstream ss(line); + std::string cell; + std::vector row; + + while (std::getline(ss, cell, ',')) + { + row.push_back(cell); + } + + // Extract the sensor values (columns 16-24) + if (row.size() > 24) + { + // std::vector accel_sample = {std::stod(row[16]), std::stod(row[17]), std::stod(row[18])}; + // std::vector gyro_sample = {std::stod(row[19]), std::stod(row[20]), std::stod(row[21])}; + // std::vector mag_sample = {std::stod(row[22]), std::stod(row[23]), std::stod(row[24])}; + + std::vector accel_sample = {std::stod(row[16]), std::stod(row[17]), std::stod(row[18])}; + std::vector gyro_sample = {std::stod(row[19]), std::stod(row[20]), std::stod(row[21])}; + std::vector mag_sample = {std::stod(row[24]), std::stod(row[25]), std::stod(row[26])}; + std::vector gps_sample = {std::stod(row[34]), std::stod(row[35])}; + std::vector altitude_sample = {std::stod(row[9])}; + std::vector time_sample = {std::stod(row[4])}; + + + // std::vector accel_scaled = { + // accel_sample[0] * 9.81, + // accel_sample[1] * 9.81, + // accel_sample[2] * 9.81}; + std::vector accel_scaled = { + accel_sample[0] *1, + accel_sample[1] *1 , + accel_sample[2] *-1}; + accel_pull.push_back(accel_scaled); + gyro_pull.push_back(gyro_sample); + mag_pull.push_back(mag_sample); + gps_pull.push_back(gps_sample); + altitude_pull.push_back(altitude_sample); + time_pull.push_back(time_sample); + } + } + file.close(); + + std::ofstream outfile("../output/mqekf_quaternion_output.csv", std::ios::out | std::ios::trunc); + + if (!outfile.is_open()) + { + std::cerr << "Failed to open file!" << std::endl; + return 1; + } + // outfile << quat[0] << "," << quat[1] << "," << quat[2] << "," << quat[3] << "\n"; + if (accel_pull.size() != gyro_pull.size() || accel_pull.size() != mag_pull.size()) + { + std::cerr << "Sensor data size mismatch!" << std::endl; + return 1; + } + + + + quat = mekf.quaternion(); + + Eigen::Matrix bias = mekf.gyroscope_bias(); + + outfile.close(); +} diff --git a/MIDAS/src/gnc/example_kf.cpp b/MIDAS/src/gnc/old_filters/example_kf.cpp similarity index 93% rename from MIDAS/src/gnc/example_kf.cpp rename to MIDAS/src/gnc/old_filters/example_kf.cpp index 1b1b3d5c..ccfa6775 100644 --- a/MIDAS/src/gnc/example_kf.cpp +++ b/MIDAS/src/gnc/old_filters/example_kf.cpp @@ -10,7 +10,7 @@ void ExampleKalmanFilter::priori() { P_priori = (F_mat * P_k * F_mat.transpose()) + Q; } -void ExampleKalmanFilter::update(Barometer barometer, Acceleration acceleration, AngularKalmanData angularkalman, FSMState FSM_state) {} +void ExampleKalmanFilter::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState current_stat) {} void ExampleKalmanFilter::setQ(float dt, float sd) {} diff --git a/MIDAS/src/gnc/example_kf.h b/MIDAS/src/gnc/old_filters/example_kf.h similarity index 92% rename from MIDAS/src/gnc/example_kf.h rename to MIDAS/src/gnc/old_filters/example_kf.h index 928c17e2..ba28f69d 100644 --- a/MIDAS/src/gnc/example_kf.h +++ b/MIDAS/src/gnc/old_filters/example_kf.h @@ -11,7 +11,7 @@ class ExampleKalmanFilter : public KalmanFilter<9, 4> void initialize(RocketSystems* args) override; //virtual void initialize(Orientation &orientation, Barometer &barometer, Acceleration &Acceleration); void priori() override; - void update(Barometer barometer, Acceleration acceleration, AngularKalmanData angularkalman, FSMState FSM_state) override; + void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState current_state) override; KalmanData getState() override; void setState(KalmanState state) override; diff --git a/MIDAS/src/gnc/yessir.cpp b/MIDAS/src/gnc/old_filters/yessir.cpp similarity index 93% rename from MIDAS/src/gnc/yessir.cpp rename to MIDAS/src/gnc/old_filters/yessir.cpp index 6d3336a4..9aef174c 100644 --- a/MIDAS/src/gnc/yessir.cpp +++ b/MIDAS/src/gnc/old_filters/yessir.cpp @@ -25,17 +25,18 @@ Yessir::Yessir() : KalmanFilter() { */ void Yessir::initialize(RocketSystems* args) { - AngularKalmanData angular_kalman = args->rocket_data.angular_kalman_data.getRecentUnsync(); + Orientation orientation = args->rocket_data.orientation.getRecentUnsync(); float sum = 0; for (int i = 0; i < 30; i++) { Barometer barometer = args->rocket_data.barometer.getRecent(); - IMU initial_accelerometer = args->rocket_data.imu.getRecent(); //lowg + LowGData initial_accelerometer = args->rocket_data.low_g.getRecent(); Acceleration accelerations = { - .ax = initial_accelerometer.lowg_acceleration.ax, - .ay = initial_accelerometer.lowg_acceleration.ay, - .az = initial_accelerometer.lowg_acceleration.az}; //please check axis divij + .ax = initial_accelerometer.ax, + .ay = initial_accelerometer.ay, + .az = initial_accelerometer.az + }; sum += barometer.altitude; init_accel(0, 0) += accelerations.az; @@ -48,7 +49,7 @@ void Yessir::initialize(RocketSystems* args) { init_accel(1, 0) /= 30; init_accel(2, 0) /= 30; - euler_t euler = angular_kalman.getEuler(); + euler_t euler = orientation.getEuler(); euler.yaw = -euler.yaw; world_accel = BodyToGlobal(euler, init_accel); @@ -138,7 +139,7 @@ void Yessir::priori() { * the new sensor data is. After updating the gain, the state estimate is updated. * */ -void Yessir::update(Barometer barometer, Acceleration acceleration, AngularKalmanData angularkalman, FSMState FSM_state) { +void Yessir::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state) { if (FSM_state == FSMState::STATE_FIRST_BOOST || FSM_state == FSMState::STATE_SECOND_BOOST) { float sum = 0; float data[10]; @@ -164,7 +165,7 @@ void Yessir::update(Barometer barometer, Acceleration acceleration, AngularKalma accel(1, 0) = acceleration.ay - 0.065; accel(2, 0) = -acceleration.ax - 0.06; - euler_t angles = angularkalman.getEuler(); + euler_t angles = orientation.getEuler(); angles.yaw = -angles.yaw; Eigen::Matrix acc = BodyToGlobal(angles, accel); @@ -208,14 +209,12 @@ void Yessir::update(Barometer barometer, Acceleration acceleration, AngularKalma * @param &orientation Current orientation * @param current_state Current FSM_state */ - - -void Yessir::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, IMU &imudata, AngularKalmanData &angularkalman, FSMState FSM_state) { +void Yessir::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState FSM_state) { if (FSM_state >= FSMState::STATE_IDLE) { setF(dt / 1000); setQ(dt / 1000, sd); priori(); - update(barometer, acceleration, angularkalman, FSM_state); + update(barometer, acceleration, orientation, FSM_state); } } diff --git a/MIDAS/src/gnc/yessir.h b/MIDAS/src/gnc/old_filters/yessir.h similarity index 87% rename from MIDAS/src/gnc/yessir.h rename to MIDAS/src/gnc/old_filters/yessir.h index 9e78212f..798b9738 100644 --- a/MIDAS/src/gnc/yessir.h +++ b/MIDAS/src/gnc/old_filters/yessir.h @@ -14,7 +14,7 @@ class Yessir : public KalmanFilter Yessir(); void initialize(RocketSystems* args) override; void priori() override; - void update(Barometer barometer, Acceleration acceleration, AngularKalmanData angularkalman, FSMState FSM_state) override; + void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState state) override; void setQ(float dt, float sd); void setF(float dt); @@ -23,7 +23,7 @@ class Yessir : public KalmanFilter KalmanData getState() override; void setState(KalmanState state) override; - void tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, IMU &imudata, AngularKalmanData &angularkalman, FSMState FSM_state); + void tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState state); private: float s_dt = 0.05f; diff --git a/MIDAS/src/gnc/rotation.h b/MIDAS/src/gnc/rotation.h index dd5365b4..0a27c803 100644 --- a/MIDAS/src/gnc/rotation.h +++ b/MIDAS/src/gnc/rotation.h @@ -1,6 +1,13 @@ #include /**************************** ROTATION FUNCTIONS ****************************/ +// Used in ekf.cpp for ECEF and ENU conversions +#define A 6378137.0 // Equatorial radius +#define F (1.0 / 298.257223563) // Flattening factor +#define B (A * (1 - F)) // Polar radius +#define E_SQ ((A * A - B * B) / (A * A)) // Eccentricity squared +#define pi 3.1415 + /** * @brief Converts a vector in the body frame to the global frame * @@ -9,31 +16,27 @@ * @return Eigen::Matrix Rotated vector in the global frame */ template -void BodyToGlobal(Angles& angles_rad, Eigen::Matrix &body_vec) +void BodyToGlobal(Angles &angles_rad, Eigen::Matrix &body_vec) { Eigen::Matrix3f roll, pitch, yaw; - roll << cos(angles_rad.roll), -sin(angles_rad.roll), 0., - sin(angles_rad.roll), cos(angles_rad.roll), 0., - 0., 0., 1.; + 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); + pitch << cos(angles_rad.pitch), 0., sin(angles_rad.pitch), + 0., 1., 0., + -sin(angles_rad.pitch), 0., cos(angles_rad.pitch); -yaw << 1., 0., 0., - 0., cos(angles_rad.yaw), -sin(angles_rad.yaw), - 0., sin(angles_rad.yaw), cos(angles_rad.yaw); + 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; + // temp = body_vec expressed in global frame (rotation_matrix * body_vec) Eigen::Vector3f temp = rotation_matrix * body_vec; - - // Convert from Z-up convention to X-up convention - // Eigen::Vector3f corrected; - // corrected(0) = temp(2); // Z → X - // corrected(1) = temp(1); // X → Y - // corrected(2) = temp(0); // Y → Z - - // body_vec = corrected; + body_vec(0, 0) = temp(0); + body_vec(1, 0) = temp(1); + body_vec(2, 0) = temp(2); } /** @@ -45,31 +48,92 @@ yaw << 1., 0., 0., * @return Eigen::Matrix Rotated vector in the body frame */ template -void GlobalToBody(Angles& angles_rad, Eigen::Matrix &global_vec) +void GlobalToBody(Angles &angles_rad, Eigen::Matrix &global_vec) { Eigen::Matrix3f roll, pitch, yaw; - + roll << cos(angles_rad.roll), -sin(angles_rad.roll), 0., - sin(angles_rad.roll), cos(angles_rad.roll), 0., - 0., 0., 1.; + sin(angles_rad.roll), cos(angles_rad.roll), 0., + 0., 0., 1.; -// Pitch about Y (tilt forward/back) -pitch << cos(angles_rad.pitch), 0., sin(angles_rad.pitch), - 0., 1., 0., + // Pitch about Y (tilt forward/back) + pitch << cos(angles_rad.pitch), 0., sin(angles_rad.pitch), + 0., 1., 0., -sin(angles_rad.pitch), 0., cos(angles_rad.pitch); -// Yaw about X (turn around up axis) -yaw << 1., 0., 0., + // Yaw about X (turn around up axis) + yaw << 1., 0., 0., 0., cos(angles_rad.yaw), -sin(angles_rad.yaw), - 0., sin(angles_rad.yaw), cos(angles_rad.yaw); + 0., sin(angles_rad.yaw), cos(angles_rad.yaw); Eigen::Matrix3f rotation_matrix = yaw * pitch * roll; Eigen::Vector3f temp = rotation_matrix.transpose() * global_vec; Eigen::Matrix3f R_zup_to_xup; - R_zup_to_xup << 1, 0,0 , - 0, 1, 0, - 0, 0, 1; + R_zup_to_xup << 1, 0, 0, + 0, 1, 0, + 0, 0, 1; global_vec = (R_zup_to_xup * rotation_matrix).transpose() * global_vec; +} + +/** + * @brief Converts GPS (degrees) to ECEF (meters) + * @return Vector of ECEF coordinates {X, Y, Z} + */ +inline std::vector gps_to_ecef(float lat, float lon, float alt) { + // Convert to radians + lat *= pi / 180.0; + lon *= pi / 180.0; + + double N = A / std::sqrt(1 - E_SQ * std::sin(lat) * std::sin(lat)); + + float ecef_x = (N + alt) * std::cos(lat) * std::cos(lon); + float ecef_y = (N + alt) * std::cos(lat) * std::sin(lon); + float ecef_z = ((1 - E_SQ) * N + alt) * std::sin(lat); + + return {ecef_x, ecef_y, ecef_z}; +} + +/** + * @brief Converts the current ECEF (meters) to ENU (meters) based on a reference in both ECEF (meters) and GPS (degrees) + * @return Vector of ENU coordinates {East, North, Up} + */ +inline std::vector ecef_to_enu(std::vector curr_ecef, std::vector ref_ecef, std::vector ref_gps) { + float ref_lat = ref_gps[0] * pi / 180.0; + float ref_lon = ref_gps[1] * pi / 180.0; + + float dx = curr_ecef[0] - ref_ecef[0]; + float dy = curr_ecef[1] - ref_ecef[1]; + float dz = curr_ecef[2] - ref_ecef[2]; + + float east = - std::sin(ref_lon) * dx + + std::cos(ref_lon) * dy; + float north = - std::sin(ref_lat) * std::cos(ref_lon) * dx + - std::sin(ref_lat) * std::sin(ref_lon) * dy + + std::cos(ref_lat) * dz; + float up = std::cos(ref_lat) * std::cos(ref_lon) * dx + + std::cos(ref_lat) * std::sin(ref_lon) * dy + + std::sin(ref_lat) * dz; + + return {east, north, up}; +} + +// void eulerToQuaternion( +// float roll, +// float pitch, +// float yaw, +// Eigen::Matrix &quat) +// { + +// float cr = cos(roll * 0.5f); +// float sr = sin(roll * 0.5f); +// float cp = cos(pitch * 0.5f); +// float sp = sin(pitch * 0.5f); +// float cy = cos(yaw * 0.5f); +// float sy = sin(yaw * 0.5f); -} \ No newline at end of file +// quat(0, 0) = cy * cp * cr + sy * sp * sr; +// quat(1, 0) = cy * cp * sr - sy * sp * cr; +// quat(2, 0) = sy * cp * sr + cy * sp * cr; +// quat(3, 0) = sy * cp * cr - cy * sp * sr; +// } \ No newline at end of file diff --git a/MIDAS/src/sensor_data.h b/MIDAS/src/sensor_data.h index edf80fa4..77cb7512 100644 --- a/MIDAS/src/sensor_data.h +++ b/MIDAS/src/sensor_data.h @@ -279,7 +279,7 @@ struct KalmanData { */ struct AngularKalmanData { Quaternion quaternion; - uint16_t gyrobias[3]; + float gyrobias[3]; double comp_tilt = 0.0; double mq_tilt = 0.0; bool has_data = false; diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index 07109059..537237cf 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -2,6 +2,7 @@ #include "sensor_data.h" #include "hal.h" #include "gnc/ekf.h" +#include "gnc/mqekf.h" #include @@ -19,9 +20,11 @@ * * The `DECLARE_THREAD` macro creates a function whose name is suffixed by _thread, and annotates it with [[noreturn]] */ -DECLARE_THREAD(logger, RocketSystems* arg) { +DECLARE_THREAD(logger, RocketSystems *arg) +{ log_begin(arg->log_sink); - while (true) { + while (true) + { log_data(arg->log_sink, arg->rocket_data); arg->rocket_data.log_latency.tick(); @@ -30,24 +33,27 @@ DECLARE_THREAD(logger, RocketSystems* arg) { } } -DECLARE_THREAD(barometer, RocketSystems* arg) { +DECLARE_THREAD(barometer, RocketSystems *arg) +{ // Reject single rogue barometer readings that are very different from the immediately prior reading // Will only reject a certain number of readings in a row Barometer prev_reading; - constexpr float altChgThreshold = 200; // meters + constexpr float altChgThreshold = 200; // meters constexpr float presChgThreshold = 500; // milibars - constexpr float tempChgThreshold = 10; // degrees C + constexpr float tempChgThreshold = 10; // degrees C constexpr unsigned int maxConsecutiveRejects = 3; unsigned int rejects = maxConsecutiveRejects; // Always accept first reading - while (true) { + while (true) + { Barometer reading = arg->sensors.barometer.read(); bool is_rogue = std::abs(prev_reading.altitude - reading.altitude) > altChgThreshold; - //std::abs(prev_reading.pressure - reading.pressure) > presChgThreshold || - //std::abs(prev_reading.temperature - reading.temperature) > tempChgThreshold; + // std::abs(prev_reading.pressure - reading.pressure) > presChgThreshold || + // std::abs(prev_reading.temperature - reading.temperature) > tempChgThreshold; // TODO: Log when we receive a rejection! if (is_rogue && rejects++ < maxConsecutiveRejects) arg->rocket_data.barometer.update(prev_reading); // Reuse old reading, reject new reading - else { + else + { rejects = 0; arg->rocket_data.barometer.update(reading); prev_reading = reading; // Only update prev_reading with accepted readings @@ -62,10 +68,11 @@ DECLARE_THREAD(barometer, RocketSystems* arg) { } } +DECLARE_THREAD(imuthread, RocketSystems *arg) +{ // This needs edits + while (true) + { -DECLARE_THREAD(imuthread, RocketSystems* arg) { //This needs edits - while (true) { - IMU imudata = arg->sensors.imu.read(); arg->rocket_data.imu.update(imudata); IMU_SFLP hw_filter = arg->sensors.imu.read_sflp(); @@ -75,57 +82,64 @@ DECLARE_THREAD(imuthread, RocketSystems* arg) { //This needs edits } } -//read angular velocity and update quaternion data -//handle mode switching from on pad madgwick to ekf? -//quaternion -> euler for telem -//orientation struct updated -// DECLARE_THREAD(orientation, RocketSystems* arg) { -// while (true) { -// Orientation orientation_holder = arg->rocket_data.orientation.getRecent(); -// Orientation reading = arg->sensors.orientation.read(); -// if (reading.has_data) { -// if(reading.reading_type == OrientationReadingType::ANGULAR_VELOCITY_UPDATE) { -// orientation_holder.angular_velocity.vx = reading.angular_velocity.vx; -// orientation_holder.angular_velocity.vy = reading.angular_velocity.vy; -// orientation_holder.angular_velocity.vz = reading.angular_velocity.vz; -// } else { -// float old_vx = orientation_holder.angular_velocity.vx; -// float old_vy = orientation_holder.angular_velocity.vy; -// float old_vz = orientation_holder.angular_velocity.vz; -// orientation_holder = reading; -// orientation_holder.angular_velocity.vx = old_vx; -// orientation_holder.angular_velocity.vy = old_vy; -// orientation_holder.angular_velocity.vz = old_vz; -// } -// arg->rocket_data.orientation.update(orientation_holder); -// } -// THREAD_SLEEP(5); -// } -// } - -DECLARE_THREAD(magnetometer, RocketSystems* arg) { - while (true) { +// read angular velocity and update quaternion data +// handle mode switching from on pad madgwick to ekf? +// quaternion -> euler for telem +// orientation struct updated +// DECLARE_THREAD(orientation, RocketSystems* arg) { +// while (true) { +// Orientation orientation_holder = arg->rocket_data.orientation.getRecent(); +// Orientation reading = arg->sensors.orientation.read(); +// if (reading.has_data) { +// if(reading.reading_type == OrientationReadingType::ANGULAR_VELOCITY_UPDATE) { +// orientation_holder.angular_velocity.vx = reading.angular_velocity.vx; +// orientation_holder.angular_velocity.vy = reading.angular_velocity.vy; +// orientation_holder.angular_velocity.vz = reading.angular_velocity.vz; +// } else { +// float old_vx = orientation_holder.angular_velocity.vx; +// float old_vy = orientation_holder.angular_velocity.vy; +// float old_vz = orientation_holder.angular_velocity.vz; +// orientation_holder = reading; +// orientation_holder.angular_velocity.vx = old_vx; +// orientation_holder.angular_velocity.vy = old_vy; +// orientation_holder.angular_velocity.vz = old_vz; +// } +// arg->rocket_data.orientation.update(orientation_holder); +// } +// THREAD_SLEEP(5); +// } +// } + +DECLARE_THREAD(magnetometer, RocketSystems *arg) +{ + while (true) + { Magnetometer reading = arg->sensors.magnetometer.read(); arg->rocket_data.magnetometer.update(reading); THREAD_SLEEP(50); } } -DECLARE_THREAD(gps, RocketSystems* arg) { - while(true) { - if(arg->sensors.gps.valid()) { +DECLARE_THREAD(gps, RocketSystems *arg) +{ + while (true) + { + if (arg->sensors.gps.valid()) + { GPS reading = arg->sensors.gps.read(); arg->rocket_data.gps.update(reading); } - //GPS waits internally + // GPS waits internally THREAD_SLEEP(1); } } -DECLARE_THREAD(pyro, RocketSystems* arg) { - while(true) { +DECLARE_THREAD(pyro, RocketSystems *arg) +{ + while (true) + { FSMState current_state = arg->rocket_data.fsm_state.getRecentUnsync(); - CommandFlags& command_flags = arg->rocket_data.command_flags; + CommandFlags &command_flags = arg->rocket_data.command_flags; PyroState new_pyro_state = arg->sensors.pyro.tick(current_state, arg->rocket_data.angular_kalman_data.getRecentUnsync(), command_flags); arg->rocket_data.pyro.update(new_pyro_state); @@ -133,13 +147,16 @@ DECLARE_THREAD(pyro, RocketSystems* arg) { arg->led.update(); THREAD_SLEEP(10); - } + } } -DECLARE_THREAD(voltage, RocketSystems* arg) { - while (true) { +DECLARE_THREAD(voltage, RocketSystems *arg) +{ + while (true) + { Continuity reading = arg->sensors.continuity.read(); - Voltage reading2 = arg->sensors.voltage.read();; + Voltage reading2 = arg->sensors.voltage.read(); + ; arg->rocket_data.continuity.update(reading); arg->rocket_data.voltage.update(reading2); @@ -149,45 +166,55 @@ DECLARE_THREAD(voltage, RocketSystems* arg) { } // This thread has a bit of extra logic since it needs to play a tune exactly once the sustainer ignites -DECLARE_THREAD(fsm, RocketSystems* arg) { +DECLARE_THREAD(fsm, RocketSystems *arg) +{ FSM fsm{}; bool already_played_freebird = false; double last_time_led_flash = pdTICKS_TO_MS(xTaskGetTickCount()); - while (true) { + while (true) + { FSMState current_state = arg->rocket_data.fsm_state.getRecentUnsync(); StateEstimate state_estimate(arg->rocket_data); - CommandFlags& telemetry_commands = arg->rocket_data.command_flags; + CommandFlags &telemetry_commands = arg->rocket_data.command_flags; double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); FSMState next_state = fsm.tick_fsm(current_state, state_estimate, telemetry_commands); arg->rocket_data.fsm_state.update(next_state); - if (current_state == FSMState::STATE_SAFE) { - if((current_time - last_time_led_flash) > 250) { + if (current_state == FSMState::STATE_SAFE) + { + if ((current_time - last_time_led_flash) > 250) + { // Flashes green LED at 4Hz while in SAFE mode. last_time_led_flash = current_time; arg->led.toggle(LED::GREEN); } - } else { + } + else + { arg->led.set(LED::GREEN, LOW); } - if ((current_state == FSMState::STATE_PYRO_TEST || current_state == FSMState::STATE_IDLE) && !arg->buzzer.is_playing()) { + if ((current_state == FSMState::STATE_PYRO_TEST || current_state == FSMState::STATE_IDLE) && !arg->buzzer.is_playing()) + { arg->buzzer.play_tune(warn_tone, WARN_TONE_LENGTH); } - if (current_state == FSMState::STATE_LANDED && !arg->buzzer.is_playing()) { + if (current_state == FSMState::STATE_LANDED && !arg->buzzer.is_playing()) + { arg->buzzer.play_tune(land_tone, LAND_TONE_LENGTH); } - if (current_state == FSMState::STATE_SUSTAINER_IGNITION && !already_played_freebird) { + if (current_state == FSMState::STATE_SUSTAINER_IGNITION && !already_played_freebird) + { arg->buzzer.play_tune(free_bird, FREE_BIRD_LENGTH); already_played_freebird = true; } - + // FSM-based camera control - if(arg->rocket_data.command_flags.FSM_should_set_cam_feed_cam1) { + if (arg->rocket_data.command_flags.FSM_should_set_cam_feed_cam1) + { // Swap camera feed to MUX 1 (Side-facing camera) at launch. arg->rocket_data.command_flags.FSM_should_set_cam_feed_cam1 = false; arg->b2b.camera.vmux_set(SIDE_CAMERA); @@ -196,13 +223,15 @@ DECLARE_THREAD(fsm, RocketSystems* arg) { arg->b2b.camera.camera_on(BULKHEAD_CAMERA); } - if(arg->rocket_data.command_flags.FSM_should_power_save) { + if (arg->rocket_data.command_flags.FSM_should_power_save) + { arg->rocket_data.command_flags.FSM_should_power_save = false; arg->b2b.camera.vtx_off(); } - if(arg->rocket_data.command_flags.FSM_should_swap_camera_feed) { + if (arg->rocket_data.command_flags.FSM_should_swap_camera_feed) + { // Swap camera feed to MUX 2 (recovery bay camera) arg->rocket_data.command_flags.FSM_should_swap_camera_feed = false; arg->b2b.camera.vmux_set(BULKHEAD_CAMERA); @@ -212,39 +241,80 @@ DECLARE_THREAD(fsm, RocketSystems* arg) { } } -DECLARE_THREAD(buzzer, RocketSystems* arg) { - while (true) { +DECLARE_THREAD(buzzer, RocketSystems *arg) +{ + while (true) + { arg->buzzer.tick(); THREAD_SLEEP(10); } } +// angularkalmandata needs updates +DECLARE_THREAD(angularkalman, RocketSystems *arg) +{ // + mqekf.initialize(arg); + // Serial.println("Initialized ekf :("); + TickType_t last = xTaskGetTickCount(); + + while (true) + { + FSMState FSM_state = arg->rocket_data.fsm_state.getRecent(); -//angularkalmandata needs updates -DECLARE_THREAD(angularkalman, RocketSystems* arg) { // - while (true) { - //Divij/Michael help here - THREAD_SLEEP(10); + if (arg->rocket_data.command_flags.should_reset_kf) + { + mqekf.initialize(arg); + TickType_t last = xTaskGetTickCount(); + arg->rocket_data.command_flags.should_reset_kf = false; + } + + IMU current_imu = arg->rocket_data.imu.getRecent(); + Acceleration current_high_g = current_imu.highg_acceleration; + Acceleration current_low_g = current_imu.lowg_acceleration; + Magnetometer current_mag = arg->rocket_data.magnetometer.getRecent(); + Velocity current_angular_velocity = current_imu.angular_velocity(); // degrees + + AngularKalmanData current_angular_kalman = args->rocket_data.angular_kalman_data.getRecent() + + Acceleration current_accelerations = { + .ax = current_high_g.ax, + .ay = current_high_g.ay, + .az = current_high_g.az}; // + + float dt = pdTICKS_TO_MS(xTaskGetTickCount() - last) / 1000.0f; + float timestamp = pdTICKS_TO_MS(xTaskGetTickCount()) / 1000.0f; + + // Check with Divij + mqekf.tick(dt, current_mag, current_angular_velocity,current_accelerations); + + AngularKalmanData current_state = mqekf.getState(); + + arg->rocket_data.angular_kalman_data.update(current_state); + + last = xTaskGetTickCount(); + // Serial.println("Angular Kalman"); + THREAD_SLEEP(50); } } - -DECLARE_THREAD(kalman, RocketSystems* arg) { +DECLARE_THREAD(kalman, RocketSystems *arg) +{ ekf.initialize(arg); // Serial.println("Initialized ekf :("); TickType_t last = xTaskGetTickCount(); - - while (true) { - if(arg->rocket_data.command_flags.should_reset_kf){ + + while (true) + { + if (arg->rocket_data.command_flags.should_reset_kf) + { ekf.initialize(arg); TickType_t last = xTaskGetTickCount(); arg->rocket_data.command_flags.should_reset_kf = false; } - + Barometer current_barom_buf = arg->rocket_data.barometer.getRecent(); - IMU current_imu = arg->rocket_data.imu.getRecent(); Acceleration current_high_g = current_imu.highg_acceleration; Acceleration current_low_g = current_imu.lowg_acceleration; @@ -253,19 +323,16 @@ DECLARE_THREAD(kalman, RocketSystems* arg) { FSMState FSM_state = arg->rocket_data.fsm_state.getRecent(); - Acceleration current_accelerations = { .ax = current_high_g.ax, .ay = current_high_g.ay, - .az = current_high_g.az - };// + .az = current_high_g.az}; // float dt = pdTICKS_TO_MS(xTaskGetTickCount() - last) / 1000.0f; float timestamp = pdTICKS_TO_MS(xTaskGetTickCount()) / 1000.0f; - - //Check with Divij - ekf.tick(dt, 13.0, current_barom_buf, current_accelerations, current_imu, current_angular_kalman, FSM_state); + // Check with Divij + ekf.tick(dt, 13.0, current_barom_buf, current_accelerations, current_imu, current_angular_kalman, FSM_state); KalmanData current_state = ekf.getState(); @@ -277,88 +344,99 @@ DECLARE_THREAD(kalman, RocketSystems* arg) { } } - -void handle_tlm_command(TelemetryCommand& command, RocketSystems* arg, FSMState current_state) { +void handle_tlm_command(TelemetryCommand &command, RocketSystems *arg, FSMState current_state) +{ // maybe we should move this somewhere else but it can stay here for now - switch(command.command) { - case CommandType::RESET_KF: - arg->rocket_data.command_flags.should_reset_kf = true; - break; - case CommandType::SWITCH_TO_SAFE: - arg->rocket_data.command_flags.should_transition_safe = true; - break; - case CommandType::SWITCH_TO_PYRO_TEST: - arg->rocket_data.command_flags.should_transition_pyro_test = true; - Serial.println("Changing to pyro test"); - break; - case CommandType::SWITCH_TO_IDLE: - arg->rocket_data.command_flags.should_transition_idle = true; - break; - case CommandType::FIRE_PYRO_A: - if (current_state == FSMState::STATE_PYRO_TEST) { - arg->rocket_data.command_flags.should_fire_pyro_a = true; - } - break; - case CommandType::FIRE_PYRO_B: - if (current_state == FSMState::STATE_PYRO_TEST) { - arg->rocket_data.command_flags.should_fire_pyro_b = true; - } - break; - case CommandType::FIRE_PYRO_C: - if (current_state == FSMState::STATE_PYRO_TEST) { - arg->rocket_data.command_flags.should_fire_pyro_c = true; - } - break; - case CommandType::FIRE_PYRO_D: - if (current_state == FSMState::STATE_PYRO_TEST) { - arg->rocket_data.command_flags.should_fire_pyro_d = true; - } - break; - case CommandType::CAM_ON: - arg->b2b.camera.vtx_on(); // comes first to avoid brownouts due to high inrush current - arg->b2b.camera.camera_on(CAM_1); - arg->b2b.camera.camera_on(CAM_2); - break; - case CommandType::CAM_OFF: - arg->b2b.camera.vtx_off(); - arg->b2b.camera.camera_off(CAM_1); - arg->b2b.camera.camera_off(CAM_2); - break; - case CommandType::TOGGLE_CAM_VMUX: - arg->b2b.camera.vmux_toggle(); - break; - default: - break; // how + switch (command.command) + { + case CommandType::RESET_KF: + arg->rocket_data.command_flags.should_reset_kf = true; + break; + case CommandType::SWITCH_TO_SAFE: + arg->rocket_data.command_flags.should_transition_safe = true; + break; + case CommandType::SWITCH_TO_PYRO_TEST: + arg->rocket_data.command_flags.should_transition_pyro_test = true; + Serial.println("Changing to pyro test"); + break; + case CommandType::SWITCH_TO_IDLE: + arg->rocket_data.command_flags.should_transition_idle = true; + break; + case CommandType::FIRE_PYRO_A: + if (current_state == FSMState::STATE_PYRO_TEST) + { + arg->rocket_data.command_flags.should_fire_pyro_a = true; + } + break; + case CommandType::FIRE_PYRO_B: + if (current_state == FSMState::STATE_PYRO_TEST) + { + arg->rocket_data.command_flags.should_fire_pyro_b = true; + } + break; + case CommandType::FIRE_PYRO_C: + if (current_state == FSMState::STATE_PYRO_TEST) + { + arg->rocket_data.command_flags.should_fire_pyro_c = true; + } + break; + case CommandType::FIRE_PYRO_D: + if (current_state == FSMState::STATE_PYRO_TEST) + { + arg->rocket_data.command_flags.should_fire_pyro_d = true; + } + break; + case CommandType::CAM_ON: + arg->b2b.camera.vtx_on(); // comes first to avoid brownouts due to high inrush current + arg->b2b.camera.camera_on(CAM_1); + arg->b2b.camera.camera_on(CAM_2); + break; + case CommandType::CAM_OFF: + arg->b2b.camera.vtx_off(); + arg->b2b.camera.camera_off(CAM_1); + arg->b2b.camera.camera_off(CAM_2); + break; + case CommandType::TOGGLE_CAM_VMUX: + arg->b2b.camera.vmux_toggle(); + break; + default: + break; // how } } -DECLARE_THREAD(cam, RocketSystems* arg) { - while (true) { +DECLARE_THREAD(cam, RocketSystems *arg) +{ + while (true) + { Wire.beginTransmission(0x69); byte error = Wire.endTransmission(); - if (error == 0) { + if (error == 0) + { arg->rocket_data.cam_data.update(arg->b2b.camera.read()); - } else { + } + else + { // If failed: CameraData new_cam_data = arg->rocket_data.cam_data.getRecent(); new_cam_data.camera_state = 255; // all 1s, invalid state arg->rocket_data.cam_data.update(new_cam_data); THREAD_SLEEP(1800); } - + THREAD_SLEEP(200); } } -DECLARE_THREAD(telemetry, RocketSystems* arg) { +DECLARE_THREAD(telemetry, RocketSystems *arg) +{ double launch_time = 0; bool has_triggered_vmux_fallback = false; - arg->rocket_data.fsm_state.update(FSMState::STATE_SAFE); - while (true) { + while (true) + { arg->tlm.transmit(arg->rocket_data, arg->led); @@ -366,12 +444,14 @@ DECLARE_THREAD(telemetry, RocketSystems* arg) { double current_time = pdTICKS_TO_MS(xTaskGetTickCount()); // This applies to STATE_SAFE, STATE_PYRO_TEST, and STATE_IDLE. - if (current_state <= FSMState::STATE_IDLE) { + if (current_state <= FSMState::STATE_IDLE) + { launch_time = current_time; has_triggered_vmux_fallback = false; } - if ((current_time - launch_time) > 79200 && !has_triggered_vmux_fallback) { + if ((current_time - launch_time) > 79200 && !has_triggered_vmux_fallback) + { // THIS IS A HARDCODED VALUE FOR AETHER II 6/21/2025 -- Value is optimal TTA from SDA // If the rocket has been in flight for over 79.2 seconds, we swap the FSM camera feed to the bulkhead camera // This is a fallback in case we can't detect the APOGEE event, so it is more conservative. @@ -379,10 +459,13 @@ DECLARE_THREAD(telemetry, RocketSystems* arg) { arg->rocket_data.command_flags.FSM_should_swap_camera_feed = true; } - if (current_state == FSMState(STATE_IDLE) || current_state == FSMState(STATE_SAFE) || current_state == FSMState(STATE_PYRO_TEST) || (current_time - launch_time) > 1800000) { + if (current_state == FSMState(STATE_IDLE) || current_state == FSMState(STATE_SAFE) || current_state == FSMState(STATE_PYRO_TEST) || (current_time - launch_time) > 1800000) + { TelemetryCommand command; - if (arg->tlm.receive(&command, 200)) { - if (command.valid()) { + if (arg->tlm.receive(&command, 200)) + { + if (command.valid()) + { arg->tlm.acknowledgeReceived(); handle_tlm_command(command, arg, current_state); } @@ -392,19 +475,28 @@ DECLARE_THREAD(telemetry, RocketSystems* arg) { } } -#define INIT_SYSTEM(s) do { ErrorCode code = (s).init(); if (code != NoError) { return code; } } while (0) +#define INIT_SYSTEM(s) \ + do \ + { \ + ErrorCode code = (s).init(); \ + if (code != NoError) \ + { \ + return code; \ + } \ + } while (0) /** * @brief Initializes all systems in order, returning early if a system's initialization process errors out. * Turns on the Orange LED while initialization is running. */ -ErrorCode init_systems(RocketSystems& systems) { +ErrorCode init_systems(RocketSystems &systems) +{ gpioDigitalWrite(LED_ORANGE, HIGH); INIT_SYSTEM(systems.sensors.imu); - //INIT_SYSTEM(systems.sensors.orientation); + // INIT_SYSTEM(systems.sensors.orientation); INIT_SYSTEM(systems.log_sink); - //INIT_SYSTEM(systems.sensors.high_g); - //INIT_SYSTEM(systems.sensors.low_g_lsm); + // INIT_SYSTEM(systems.sensors.high_g); + // INIT_SYSTEM(systems.sensors.low_g_lsm); INIT_SYSTEM(systems.sensors.barometer); INIT_SYSTEM(systems.sensors.magnetometer); INIT_SYSTEM(systems.sensors.continuity); @@ -413,36 +505,37 @@ ErrorCode init_systems(RocketSystems& systems) { INIT_SYSTEM(systems.led); INIT_SYSTEM(systems.buzzer); INIT_SYSTEM(systems.b2b); - #ifdef ENABLE_TELEM - INIT_SYSTEM(systems.tlm); - #endif +#ifdef ENABLE_TELEM + INIT_SYSTEM(systems.tlm); +#endif INIT_SYSTEM(systems.sensors.gps); gpioDigitalWrite(LED_ORANGE, LOW); return NoError; } #undef INIT_SYSTEM - /** * @brief Initializes the systems, and then creates and starts the thread for each system. * If initialization fails, then this enters an infinite loop. */ -[[noreturn]] void begin_systems(RocketSystems* config) { +[[noreturn]] void begin_systems(RocketSystems *config) +{ Serial.println("Starting Systems..."); ErrorCode init_error_code = init_systems(*config); - if (init_error_code != NoError) { + if (init_error_code != NoError) + { // todo some message probably Serial.print("Had Error: "); - Serial.print((int) init_error_code); + Serial.print((int)init_error_code); Serial.print("\n"); Serial.flush(); update_error_LED(init_error_code); - while (true) { - + while (true) + { } } - //START_THREAD(orientation, SENSOR_CORE, config, 10); + // START_THREAD(orientation, SENSOR_CORE, config, 10); START_THREAD(logger, DATA_CORE, config, 15); START_THREAD(imuthread, SENSOR_CORE, config, 13); START_THREAD(barometer, SENSOR_CORE, config, 12); @@ -455,13 +548,14 @@ ErrorCode init_systems(RocketSystems& systems) { START_THREAD(fsm, SENSOR_CORE, config, 8); START_THREAD(buzzer, SENSOR_CORE, config, 6); START_THREAD(angularkalman, SENSOR_CORE, config, 7) - #ifdef ENABLE_TELEM +#ifdef ENABLE_TELEM START_THREAD(telemetry, SENSOR_CORE, config, 15); - #endif +#endif config->buzzer.play_tune(free_bird, FREE_BIRD_LENGTH); - while (true) { + while (true) + { THREAD_SLEEP(1000); Serial.print("Running (Log Latency: "); Serial.print(config->rocket_data.log_latency.getLatency()); @@ -469,7 +563,7 @@ ErrorCode init_systems(RocketSystems& systems) { } } -//void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char* pcTaskName){ -// Serial.println("OVERFLOW"); -// Serial.println((char*)pcTaskName); -//} +// void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char* pcTaskName){ +// Serial.println("OVERFLOW"); +// Serial.println((char*)pcTaskName); +// } From b71401a5abfca9dde66623c5f1591ce304a9cb9c Mon Sep 17 00:00:00 2001 From: Divij Date: Mon, 9 Feb 2026 20:24:11 -0600 Subject: [PATCH 2/3] edited to make sure ekf has angular kalman struct Co-authored-by: lucasbm3 Co-authored-by: MichaelCyrwus Co-authored-by: Ahmed Khan Co-authored-by: Keshav B --- MIDAS/src/gnc/ekf.cpp | 12 ++++++------ MIDAS/src/gnc/ekf.h | 6 +++--- MIDAS/src/systems.cpp | 3 ++- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index 64fe39ea..e09aef0e 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -27,7 +27,7 @@ EKF::EKF() : KalmanFilter() */ void EKF::initialize(RocketSystems *args) { - Orientation orientation = args->rocket_data.orientation.getRecentUnsync(); + // Orientation orientation = args->rocket_data.orientation.getRecentUnsync(); float sum = 0; for (int i = 0; i < 30; i++) @@ -72,7 +72,7 @@ void EKF::initialize(RocketSystems *args) * it extrapolates the state at time n+1 based on the state at time n. */ -void EKF::priori(float dt, Orientation &orientation, FSMState fsm, Acceleration acceleration) +void EKF::priori(float dt, AngularKalmanData &orientation, FSMState fsm, Acceleration acceleration) { setF(dt); setB(dt); @@ -81,8 +81,8 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm, Acceleration sensor_accel_global_g(0, 0) = acceleration.ax + 0.045f; sensor_accel_global_g(1, 0) = acceleration.ay - 0.065f; sensor_accel_global_g(2, 0) = acceleration.az - 0.06f; - // euler_t angles_rad = orientation.getEuler(); - // BodyToGlobal(angles_rad, sensor_accel_global_g); + euler_t angles_rad = orientation.getEuler(); + BodyToGlobal(angles_rad, sensor_accel_global_g); // Do not apply gravity if on pad float g_ms2 = (fsm > FSMState::STATE_IDLE) ? gravity_ms2 : 0.0f; u_control(0, 0) = sensor_accel_global_g(0, 0) * g_ms2; @@ -100,7 +100,7 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm, Acceleration * Updates with barometer (always) and GPS (if available). * */ -void EKF::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state, GPS &gps) +void EKF::update(Barometer barometer, Acceleration acceleration, AngularKalmanData orientation, FSMState FSM_state, GPS &gps) { // if on pad take last 10 barometer measurements for init state if (FSM_state == FSMState::STATE_IDLE) @@ -234,7 +234,7 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori * @param &orientation Current orientation * @param current_state Current FSM_state */ -void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState FSM_state, GPS &gps) +void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, AngularKalmanData &orientation, FSMState FSM_state, GPS &gps) { if (FSM_state >= FSMState::STATE_IDLE) // diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index 6c59e8da..e991f7bc 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -22,8 +22,8 @@ class EKF : public KalmanFilter EKF(); void initialize(RocketSystems* args) override; // void priori(); - void priori(float dt, Orientation &orientation, FSMState fsm, Acceleration acceleration); - void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState state, GPS &gps) override; + void priori(float dt, AngularKalmanData &orientation, FSMState fsm, Acceleration acceleration); + void update(Barometer barometer, Acceleration acceleration, AngularKalmanData orientation, FSMState state, GPS &gps) override; void setQ(float dt, float sd); void setF(float dt); @@ -33,7 +33,7 @@ class EKF : public KalmanFilter void setState(KalmanState state) override; void reference_GPS(GPS &gps, FSMState fsm); - void tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState state, GPS &gps); + void tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, AngularKalmanData &orientation, FSMState state, GPS &gps); bool should_reinit = false; float current_vel = 0.0f; diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index 537237cf..010806c9 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -322,6 +322,7 @@ DECLARE_THREAD(kalman, RocketSystems *arg) AngularKalmanData current_angular_kalman = args->rocket_data.angular_kalman_data.getRecent() FSMState FSM_state = arg->rocket_data.fsm_state.getRecent(); + GPS current_gps = arg->rocket_data.gps.getRecent(); Acceleration current_accelerations = { .ax = current_high_g.ax, @@ -332,7 +333,7 @@ DECLARE_THREAD(kalman, RocketSystems *arg) float timestamp = pdTICKS_TO_MS(xTaskGetTickCount()) / 1000.0f; // Check with Divij - ekf.tick(dt, 13.0, current_barom_buf, current_accelerations, current_imu, current_angular_kalman, FSM_state); + ekf.tick(dt, 13.0, current_barom_buf, current_accelerations, current_imu, current_angular_kalman, FSM_state, current_gps); KalmanData current_state = ekf.getState(); From 26fcaa39ca37be9fbe9cdc4d109e62fb4e126091 Mon Sep 17 00:00:00 2001 From: Divij Date: Wed, 11 Feb 2026 02:00:14 -0600 Subject: [PATCH 3/3] moved values to constants --- MIDAS/src/gnc/constants.h | 5 +++++ MIDAS/src/gnc/mqekf.cpp | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/MIDAS/src/gnc/constants.h b/MIDAS/src/gnc/constants.h index 76525104..f00a7e9d 100644 --- a/MIDAS/src/gnc/constants.h +++ b/MIDAS/src/gnc/constants.h @@ -13,3 +13,8 @@ const float mass_full = mass_sustainer_wet+mass_booster_wet; // (kg) Total mass const float mass_first_burnout = mass_booster_dry+mass_sustainer_wet;// (kg) Total mass after first stage burnout const float mass_second_burnout = mass_booster_dry+mass_sustainer_dry;// (kg) Total mass after first stage burnout const float gravity_ms2 = 9.81; // (m/s^2) accel due to gravity +const float accel_noise_density_x = 1000; // ug/sqrt(hz) from the accelerometer on MIDAS MINI. Assuming Acceleration noise density (high-g) in high-performance mode +const float accel_noise_density_y = 1000; // ug/sqrt(hz) from the accelerometer on MIDAS MINI +const float accel_noise_density_z = 1000; // ug/sqrt(hz) from the accelerometer on MIDAS MINI +const float gyro_RMS_noise = 0.1; // Need to get this data from the datasheet +const float mag_RMS_noise = ; //mG \ No newline at end of file diff --git a/MIDAS/src/gnc/mqekf.cpp b/MIDAS/src/gnc/mqekf.cpp index 18855d69..ee2c8a20 100644 --- a/MIDAS/src/gnc/mqekf.cpp +++ b/MIDAS/src/gnc/mqekf.cpp @@ -21,7 +21,7 @@ void QuaternionMEKF::initialize(RocketSystems *args) Eigen::Matrix sigmas; sigmas << sigma_a, sigma_m; - sigma_a = {160 * sqrt(100.0f) * 1e-6 * 9.81, 160 * sqrt(100.0f) * 1e-6 * 9.81, 190 * sqrt(100.0f) * 1e-6 * 9.81}; // ug/sqrt(Hz) *sqrt(hz). values are from datasheet + sigma_a = {accel_noise_density_x * sqrt(100.0f) * 1e-6 * 9.81, accel_noise_density_y * sqrt(100.0f) * 1e-6 * 9.81, accel_noise_density_z * sqrt(100.0f) * 1e-6 * 9.81}; // ug/sqrt(Hz) *sqrt(hz). values are from datasheet sigma_g = {0.1 * pi / 180, 0.1 * pi / 180, 0.1 * pi / 180}; // 0.1 deg/s sigma_m = {0.4e-4 / sqrt(3), 0.4e-4 / sqrt(3), 0.4e-4 / sqrt(3)}; // 0.4 mG -> T, it is 0.4 total so we divide by sqrt3