From 5a89cbc2abd69f80a5dcfac517805d800d515c89 Mon Sep 17 00:00:00 2001 From: Divij Date: Wed, 25 Feb 2026 21:10:25 -0600 Subject: [PATCH 1/6] updated only ekf for the midas mk2 software, not including mqekf --- MIDAS/src/gnc/constants.h | 8 +- MIDAS/src/gnc/ekf.cpp | 557 ++++++++++++-------------------------- MIDAS/src/gnc/ekf.h | 46 ++-- MIDAS/src/gnc/rotation.h | 128 ++++++--- MIDAS/src/systems.cpp | 8 +- 5 files changed, 299 insertions(+), 448 deletions(-) diff --git a/MIDAS/src/gnc/constants.h b/MIDAS/src/gnc/constants.h index 57295e30..6dad5282 100644 --- a/MIDAS/src/gnc/constants.h +++ b/MIDAS/src/gnc/constants.h @@ -1,10 +1,4 @@ // 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.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 diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index 5b05d1eb..1337e373 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -1,20 +1,9 @@ #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(); } - -/** - * 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 @@ -26,8 +15,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 @@ -43,92 +30,40 @@ void EKF::initialize(RocketSystems *args) for (int i = 0; i < 30; i++) { Barometer barometer = args->rocket_data.barometer.getRecent(); - LowGData initial_accelerometer = args->rocket_data.low_g.getRecent(); - Acceleration accelerations = { - .ax = initial_accelerometer.ax, - .ay = initial_accelerometer.ay, - .az = initial_accelerometer.az}; 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 = orientation.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_; - - // Wind vector - // Wind(0, 0) = 0.0; // wind in x direction - // Wind(1, 0) = 0.0; // wind in y direction - // Wind(2, 0) = 0.0; // wind in z direction + 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; + 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) = 0.1f; // barometer noise (m) + R(1, 1) = 1.5f; // GPS altitude noise (m) + R(2, 2) = 3.0f; // GPS east noise (m) + R(3, 3) = 3.0f; // GPS north noise (m). Converted 0.3 deg heading accuracies roughly to meters, needs checking post launch. +} - // set B (don't care about what's in B since we have no control input) - B(2, 0) = -1; +void EKF::priori() +{ + // For building purposes } /** @@ -139,125 +74,46 @@ 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) +void EKF::priori(float dt, Orientation &orientation, FSMState fsm, Acceleration acceleration) { - Eigen::Matrix xdot = Eigen::Matrix::Zero(); - - // angular states from sensors - Velocity omega_rps = orientation.getAngularVelocity(); // rads per sec - - euler_t angles_rad = orientation.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; + // Rotate body-frame accel to global; BodyToGlobal outputs (North, East, Down), state is (Up, East, North) + // euler_t angles_rad = orientation.getEuler(); + // BodyToGlobal(angles_rad, sensor_accel_global_g); + // float north = sensor_accel_global_g(0, 0), east = sensor_accel_global_g(1, 0), down = sensor_accel_global_g(2, 0); + // sensor_accel_global_g(0, 0) = -down; // up + // sensor_accel_global_g(1, 0) = east; + // sensor_accel_global_g(2, 0) = north; + // 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; } - +void EKF::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState current_state) +{ + // for building purposes +} /** - * @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, Orientation orientation, 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; @@ -267,87 +123,88 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori { 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; - euler_t angles_rad = orientation.getEuler(); - // angles_rad.yaw = -angles_rad.yaw; // coordinate frame match + // Update GPS reference point if in IDLE state (needed before GPS measurement update) + reference_GPS(gps, FSM_state); - 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; - + // 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}; - - // if (FSM_state > FSMState::STATE_IDLE) - // { - // current_vel += (s_dt)*y_k(1, 0); - // Eigen::Matrix measured_v = Eigen::Matrix::Zero(); - // measured_v(0, 0) = current_vel; - // measured_v(0,0) = y_k(1) + (dt/2)*y_k(2); - // Eigen::Matrix err = Eigen::Matrix::Zero(); - // err(0, 0) = measured_v(0, 0) - x_k(1, 0); - // Wind = Wind_alpha * Wind + (1 - Wind_alpha) * err; - // if (Wind.norm() > 15) - // { - // Wind(0, 0) = 15.0; - // Wind(1, 0) = 0.0; - // Wind(2, 0) = 0.0; - // } - // } } /** @@ -360,9 +217,9 @@ 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) +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 >= FSMState::STATE_IDLE) { if (FSM_state != last_fsm) { @@ -370,10 +227,11 @@ void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration accelerati last_fsm = FSM_state; } 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, orientation, FSM_state); - update(barometer, acceleration, orientation, FSM_state); + priori(dt, orientation, FSM_state, acceleration); + update(barometer, acceleration, orientation, FSM_state, gps); } } @@ -414,39 +272,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; + // continuous 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; } /** @@ -458,103 +303,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 fb78be83..19d2e827 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -1,16 +1,17 @@ #pragma once - #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 #define AERO_DATA_SIZE (sizeof(aero_data) / sizeof(aero_data[0])) @@ -19,22 +20,20 @@ class EKF : public KalmanFilter public: EKF(); void initialize(RocketSystems* args) override; - void priori(); - void priori(float dt, Orientation &orientation, FSMState fsm); - void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState state) override; + void priori() override; + void priori(float dt, Orientation &orientation, FSMState fsm, Acceleration acceleration); + void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState state, GPS &gps); + void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState current_state) 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, Orientation &orientation, FSMState 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,36 @@ 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; - + 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; + 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/rotation.h b/MIDAS/src/gnc/rotation.h index dd5365b4..f8aa245f 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 Flat (1.0 / 298.257223563) // Flattening factor +#define B (A * (1 - Flat)) // 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/systems.cpp b/MIDAS/src/systems.cpp index 1e8fcca6..3f9b6fdf 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -230,18 +230,18 @@ DECLARE_THREAD(kalman, RocketSystems* arg) { arg->rocket_data.command_flags.should_reset_kf = false; } // add the tick update function + GPS current_gps = arg->rocket_data.gps.getRecent(); Barometer current_barom_buf = arg->rocket_data.barometer.getRecent(); Orientation current_orientation = arg->rocket_data.orientation.getRecent(); HighGData current_accelerometer = arg->rocket_data.high_g.getRecent(); - FSMState FSM_state = arg->rocket_data.fsm_state.getRecent(); + FSMState fsm_state = arg->rocket_data.fsm_state.getRecent(); Acceleration current_accelerations = { .ax = current_accelerometer.ax, .ay = current_accelerometer.ay, .az = current_accelerometer.az }; float dt = pdTICKS_TO_MS(xTaskGetTickCount() - last) / 1000.0f; - float timestamp = pdTICKS_TO_MS(xTaskGetTickCount()) / 1000.0f; - ekf.tick(dt, 13.0, current_barom_buf, current_accelerations, current_orientation, FSM_state); + ekf.tick(dt, 13.0, current_barom_buf, current_accelerations, current_orientation, fsm_state, current_gps); KalmanData current_state = ekf.getState(); arg->rocket_data.kalman.update(current_state); @@ -251,8 +251,6 @@ DECLARE_THREAD(kalman, RocketSystems* arg) { THREAD_SLEEP(50); } } - - 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) { From cffd38238987ef39131621371d79d79043a7ca65 Mon Sep 17 00:00:00 2001 From: Divij Date: Fri, 27 Feb 2026 16:30:43 -0500 Subject: [PATCH 2/6] fixed magnetometer orientation --- MIDAS/src/esp_eeprom_checksum.h | 2 ++ MIDAS/src/hardware/Magnetometer.cpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) create mode 100644 MIDAS/src/esp_eeprom_checksum.h diff --git a/MIDAS/src/esp_eeprom_checksum.h b/MIDAS/src/esp_eeprom_checksum.h new file mode 100644 index 00000000..3c59605f --- /dev/null +++ b/MIDAS/src/esp_eeprom_checksum.h @@ -0,0 +1,2 @@ +// autogenerated on build by applying crc32 on esp_eeprom_format.h +#define EEPROM_CHECKSUM (0xcfa22d7d) diff --git a/MIDAS/src/hardware/Magnetometer.cpp b/MIDAS/src/hardware/Magnetometer.cpp index 23ef322d..7657c160 100644 --- a/MIDAS/src/hardware/Magnetometer.cpp +++ b/MIDAS/src/hardware/Magnetometer.cpp @@ -22,6 +22,6 @@ Magnetometer MagnetometerSensor::read() { float mx = LIS3MDL.x_gauss; float my = LIS3MDL.y_gauss; float mz = LIS3MDL.z_gauss; - Magnetometer reading{mx, my, mz}; + Magnetometer reading{-mx, my, -mz}; return reading; } From 088d15ca73a629eaae443612533c3a9a457cf7cf Mon Sep 17 00:00:00 2001 From: Muhammad Ali Date: Fri, 27 Feb 2026 19:24:23 -0600 Subject: [PATCH 3/6] esp_eeprom_checksum.h added to gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 0a840357..2c2907ea 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ .vscode/ outputs/* ground/outputs/* +MIDAS/src/esp_eeprom_checksum.h \ No newline at end of file From 467ab5ac96c86b258ae1a5a47cd15afa0dc0a70d Mon Sep 17 00:00:00 2001 From: Muhammad Ali Date: Fri, 27 Feb 2026 19:26:17 -0600 Subject: [PATCH 4/6] deleted the file haha --- MIDAS/src/esp_eeprom_checksum.h | 2 -- 1 file changed, 2 deletions(-) delete mode 100644 MIDAS/src/esp_eeprom_checksum.h diff --git a/MIDAS/src/esp_eeprom_checksum.h b/MIDAS/src/esp_eeprom_checksum.h deleted file mode 100644 index 3c59605f..00000000 --- a/MIDAS/src/esp_eeprom_checksum.h +++ /dev/null @@ -1,2 +0,0 @@ -// autogenerated on build by applying crc32 on esp_eeprom_format.h -#define EEPROM_CHECKSUM (0xcfa22d7d) From ee287a4c09f0bc8ee2026c887ec27a1c09f2e175 Mon Sep 17 00:00:00 2001 From: Divij Date: Fri, 27 Feb 2026 22:20:47 -0600 Subject: [PATCH 5/6] fixed BNO reference frames --- MIDAS/src/hardware/Orientation.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/MIDAS/src/hardware/Orientation.cpp b/MIDAS/src/hardware/Orientation.cpp index a160982d..5868e196 100644 --- a/MIDAS/src/hardware/Orientation.cpp +++ b/MIDAS/src/hardware/Orientation.cpp @@ -215,25 +215,25 @@ Orientation OrientationSensor::read() sensor_reading.pitch = euler.x; sensor_reading.roll = euler.z; - sensor_reading.linear_acceleration.ax = -event.un.accelerometer.y; - sensor_reading.linear_acceleration.ay = event.un.accelerometer.x; - sensor_reading.linear_acceleration.az = event.un.accelerometer.z; - + sensor_reading.linear_acceleration.ax = -event.un.accelerometer.x; + sensor_reading.linear_acceleration.ay = event.un.accelerometer.y; + sensor_reading.linear_acceleration.az = -event.un.accelerometer.z; + Velocity velocity; - velocity.vx = sensor_reading.linear_acceleration.ax * deltaTime + velocity.vx; + velocity.vx = sensor_reading.linear_acceleration.ax * deltaTime + velocity.vx; //this is bad. velocity.vx is 0 anyways. not sure what intent was here. velocity.vy = sensor_reading.linear_acceleration.ay * deltaTime + velocity.vy; velocity.vz = sensor_reading.linear_acceleration.az * deltaTime + velocity.vz; sensor_reading.orientation_velocity = velocity; - sensor_reading.gx = -event.un.gyroscope.y; - sensor_reading.gy = event.un.gyroscope.x; - sensor_reading.gz = event.un.gyroscope.z; + sensor_reading.gx = -event.un.gyroscope.x; + sensor_reading.gy = event.un.gyroscope.y; + sensor_reading.gz = -event.un.gyroscope.z; - sensor_reading.magnetometer.mx = -event.un.magneticField.y; - sensor_reading.magnetometer.my = event.un.magneticField.x; - sensor_reading.magnetometer.mz = event.un.magneticField.z; + sensor_reading.magnetometer.mx = -event.un.magneticField.x; + sensor_reading.magnetometer.my = event.un.magneticField.y; + sensor_reading.magnetometer.mz = -event.un.magneticField.z; sensor_reading.temperature = event.un.temperature.value; sensor_reading.pressure = event.un.pressure.value; From 0cd43723ea903b7be913bbda44885a1b462efcc4 Mon Sep 17 00:00:00 2001 From: Divij Date: Fri, 27 Feb 2026 22:30:44 -0600 Subject: [PATCH 6/6] quick comment --- MIDAS/src/hardware/Orientation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MIDAS/src/hardware/Orientation.cpp b/MIDAS/src/hardware/Orientation.cpp index 5868e196..2173dcd1 100644 --- a/MIDAS/src/hardware/Orientation.cpp +++ b/MIDAS/src/hardware/Orientation.cpp @@ -211,7 +211,7 @@ Orientation OrientationSensor::read() sensor_reading.roll = filtered_euler.z; */ - sensor_reading.yaw = -euler.y; + sensor_reading.yaw = -euler.y; //this is wrong, but not changing for Feb launch because it's unnecessary. sensor_reading.pitch = euler.x; sensor_reading.roll = euler.z;