From e498852e6584ac82f47508bb1cff3f7c39d6d4a3 Mon Sep 17 00:00:00 2001 From: Divij Date: Wed, 25 Feb 2026 23:25:08 -0600 Subject: [PATCH 1/7] new tilt changes --- MIDAS/src/gnc/constants.h | 16 ++-------------- MIDAS/src/gnc/ekf.cpp | 30 +++++++++++------------------- MIDAS/src/gnc/ekf.h | 1 - MIDAS/src/gnc/mqekf.cpp | 38 ++++++++++++++++++++------------------ 4 files changed, 33 insertions(+), 52 deletions(-) diff --git a/MIDAS/src/gnc/constants.h b/MIDAS/src/gnc/constants.h index 5a334bba..1ea7d18d 100644 --- a/MIDAS/src/gnc/constants.h +++ b/MIDAS/src/gnc/constants.h @@ -2,21 +2,9 @@ // 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_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 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 = 0; //mG \ No newline at end of file +const float gyro_RMS_noise =3.8; //mdps/√Hz +const float mag_noise = 0.4; //mG \ No newline at end of file diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index ef4e69c4..407ef404 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -57,8 +57,8 @@ void EKF::initialize(RocketSystems *args) 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(0, 0) = 0.1; // barometer noise + R(1, 1) = 1.5f; // GPS altitude noise (lower trust) R(2, 2) = 3.0f; // GPS east noise R(3, 3) = 3.0f; // GPS north noise } @@ -77,9 +77,9 @@ void EKF::priori(float dt, AngularKalmanData &orientation, FSMState fsm, Acceler 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; + sensor_accel_global_g(0, 0) = acceleration.ax ; + sensor_accel_global_g(1, 0) = acceleration.ay ; + sensor_accel_global_g(2, 0) = acceleration.az ; euler_t angles_rad = orientation.getEuler(); BodyToGlobal(angles_rad, sensor_accel_global_g); // Do not apply acceleration if on pad @@ -192,20 +192,12 @@ void EKF::update(Barometer barometer, Acceleration acceleration, AngularKalmanDa 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 - } + // // 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 + // } } // Update output state structure diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index e991f7bc..52b5fe95 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -47,7 +47,6 @@ class EKF : public KalmanFilter 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 diff --git a/MIDAS/src/gnc/mqekf.cpp b/MIDAS/src/gnc/mqekf.cpp index adf3e627..a6b5d11c 100644 --- a/MIDAS/src/gnc/mqekf.cpp +++ b/MIDAS/src/gnc/mqekf.cpp @@ -1,5 +1,6 @@ #include "mqekf.h" - +#include "constants.h" +#include /* mag y -> -x @@ -15,9 +16,9 @@ void QuaternionMEKF::initialize(RocketSystems *args) { float Pq0 = 1e-6; float Pb0 = 1e-1; - 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 + sigma_a = {accel_noise_density_x * sqrt(20.0f) * 1.0e-6 * 9.81, accel_noise_density_y * sqrt(20.0f) * 1.0e-6 * 9.81, accel_noise_density_z * sqrt(20.0f) * 1.0e-6 * 9.81}; // ug/sqrt(Hz) *sqrt(hz). values are from datasheet + sigma_g = {gyro_RMS_noise * pow(10.0f, -3.0f) * pi / 180.0f * pow(20.0f, 0.5f), gyro_RMS_noise * pow(10.0f, -3.0f) * pi / 180.0f * pow(20.0f, 0.5f), gyro_RMS_noise * pow(10.0f, -3.0f) * pi / 180.0f * pow(20.0f, 0.5f)}; + sigma_m = {mag_noise * 1.0e-4 / sqrt(3), mag_noise * 1.0e-4 / sqrt(3), mag_noise * 1.0e-4 / sqrt(3)}; // 0.4 mG -> T, it is 0.4 total so we divide by sqrt3 // 0.4 mG -> T, it is 0.4 total so we divide by sqrt3 Q = initialize_Q(sigma_g); Eigen::Matrix sigmas; sigmas << sigma_a, sigma_m; @@ -38,7 +39,7 @@ void QuaternionMEKF::initialize(RocketSystems *args) { IMU imu_data = args->rocket_data.imu.getRecent(); - Acceleration accel = imu_data.highg_acceleration; + Acceleration accel = imu_data.lowg_acceleration; accel_sum.ax += accel.ax; accel_sum.ay += accel.ay; accel_sum.az += accel.az; @@ -60,7 +61,7 @@ void QuaternionMEKF::initialize(RocketSystems *args) else { IMU imu_data = args->rocket_data.imu.getRecent(); - accel = imu_data.highg_acceleration; + accel = imu_data.lowg_acceleration; mag = args->rocket_data.magnetometer.getRecent(); } @@ -85,6 +86,7 @@ void QuaternionMEKF::tick(float dt, Magnetometer &magnetometer, Velocity &angula time_update(angular_velocity, dt); measurement_update(acceleration, magnetometer); + calculate_tilt(); Eigen::Matrix curr_quat = quaternion(); // w,x,y,z state.quaternion.w = curr_quat(0, 0); @@ -301,9 +303,9 @@ void QuaternionMEKF::initialize_from_acc_mag(Acceleration const &acc_struct, Mag 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(); + Eigen::Matrix const Rx = acc_normalized; + Eigen::Matrix const Rz = Rx.cross(mag_normalized).normalized(); + Eigen::Matrix const Ry = Rz.cross(Rx).normalized(); // Construct the rotation matrix Eigen::Matrix const R = (Eigen::Matrix() << Rx, Ry, Rz).finished(); @@ -359,11 +361,11 @@ void QuaternionMEKF::calculate_tilt() // Quat --> euler --> rotation matrix --> reference&cur vector --> dot product for angle! Eigen::Quaternion - ref = Eigen::Quaternionf(1, 0, 0, 0); + ref = Eigen::Quaternionf(0, 0, 0, 1); Eigen::Quaternion rotated = qref * ref * qref.conjugate(); - Eigen::Matrix reference_vector = {0, 0, -1}; + Eigen::Matrix reference_vector = {0, 0, 1}; Eigen::Matrix rotated_vector = {rotated.x(), rotated.y(), rotated.z()}; float dot = rotated_vector.dot(reference_vector); @@ -376,14 +378,14 @@ void QuaternionMEKF::calculate_tilt() tilt = acos(dot / (cur_mag * ref_mag)); } - const float gain = 0.2; - // Arthur's Comp Filter - float filtered_tilt = gain * tilt + (1 - gain) * prev_tilt; - prev_tilt = filtered_tilt; - state.mq_tilt = filtered_tilt; + // const float gain = 0.2; + // // Arthur's Comp Filter + // float filtered_tilt = gain * tilt + (1 - gain) * prev_tilt; + // prev_tilt = filtered_tilt; + state.mq_tilt = tilt; // Serial.print("TILT: "); - // Serial.println(filtered_tilt * (180/3.14f)); + Serial.println(tilt * 180.0f / pi); } -QuaternionMEKF mqekf; \ No newline at end of file +QuaternionMEKF mqekf; From b50bc163e1f4df3267f418fda7c89e5ac355dbe0 Mon Sep 17 00:00:00 2001 From: Thomas McManamen Date: Thu, 26 Feb 2026 00:00:56 -0600 Subject: [PATCH 2/7] telemeter mq tilt rather than comp tilt --- MIDAS/src/telemetry.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MIDAS/src/telemetry.cpp b/MIDAS/src/telemetry.cpp index 2bc10cf4..387a8bdb 100644 --- a/MIDAS/src/telemetry.cpp +++ b/MIDAS/src/telemetry.cpp @@ -111,7 +111,7 @@ TelemetryPacket Telemetry::makePacket(RocketData& data) { // Tilt & FSM State --> comp_tilt vs mq_tilt static_assert(FSMState::FSM_STATE_COUNT < 16); - uint16_t tilt_norm = (angular_kalman.comp_tilt / M_PI) * 0x0fff; // Encodes tilt value 0-1 into range 0x0000 - 0x0fff + uint16_t tilt_norm = (angular_kalman.mq_tilt / M_PI) * 0x0fff; // Encodes tilt value 0-1 into range 0x0000 - 0x0fff packet.tilt_fsm |= ((tilt_norm << 4) & 0xfff0); packet.tilt_fsm |= ((uint16_t)fsm & 0x000f); From 1a5daf6993045ed804c117da20f40fa2f3f3bdd3 Mon Sep 17 00:00:00 2001 From: Thomas McManamen Date: Thu, 26 Feb 2026 00:41:17 -0600 Subject: [PATCH 3/7] tilt tweaks --- MIDAS/src/gnc/mqekf.cpp | 6 +++--- MIDAS/src/hardware/Magnetometer.cpp | 2 +- MIDAS/src/systems.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/MIDAS/src/gnc/mqekf.cpp b/MIDAS/src/gnc/mqekf.cpp index a6b5d11c..85c62b02 100644 --- a/MIDAS/src/gnc/mqekf.cpp +++ b/MIDAS/src/gnc/mqekf.cpp @@ -17,7 +17,7 @@ void QuaternionMEKF::initialize(RocketSystems *args) float Pq0 = 1e-6; float Pb0 = 1e-1; sigma_a = {accel_noise_density_x * sqrt(20.0f) * 1.0e-6 * 9.81, accel_noise_density_y * sqrt(20.0f) * 1.0e-6 * 9.81, accel_noise_density_z * sqrt(20.0f) * 1.0e-6 * 9.81}; // ug/sqrt(Hz) *sqrt(hz). values are from datasheet - sigma_g = {gyro_RMS_noise * pow(10.0f, -3.0f) * pi / 180.0f * pow(20.0f, 0.5f), gyro_RMS_noise * pow(10.0f, -3.0f) * pi / 180.0f * pow(20.0f, 0.5f), gyro_RMS_noise * pow(10.0f, -3.0f) * pi / 180.0f * pow(20.0f, 0.5f)}; + sigma_g = {gyro_RMS_noise * pow(1.0f, -3.0f) * pi / 180.0f * pow(20.0f, 0.5f), gyro_RMS_noise * pow(1.0f, -3.0f) * pi / 180.0f * pow(20.0f, 0.5f), gyro_RMS_noise * pow(1.0f, -3.0f) * pi / 180.0f * pow(20.0f, 0.5f)}; sigma_m = {mag_noise * 1.0e-4 / sqrt(3), mag_noise * 1.0e-4 / sqrt(3), mag_noise * 1.0e-4 / sqrt(3)}; // 0.4 mG -> T, it is 0.4 total so we divide by sqrt3 // 0.4 mG -> T, it is 0.4 total so we divide by sqrt3 Q = initialize_Q(sigma_g); Eigen::Matrix sigmas; @@ -39,7 +39,7 @@ void QuaternionMEKF::initialize(RocketSystems *args) { IMU imu_data = args->rocket_data.imu.getRecent(); - Acceleration accel = imu_data.lowg_acceleration; + Acceleration accel = imu_data.highg_acceleration; accel_sum.ax += accel.ax; accel_sum.ay += accel.ay; accel_sum.az += accel.az; @@ -61,7 +61,7 @@ void QuaternionMEKF::initialize(RocketSystems *args) else { IMU imu_data = args->rocket_data.imu.getRecent(); - accel = imu_data.lowg_acceleration; + accel = imu_data.highg_acceleration; mag = args->rocket_data.magnetometer.getRecent(); } diff --git a/MIDAS/src/hardware/Magnetometer.cpp b/MIDAS/src/hardware/Magnetometer.cpp index 46966dd1..f9a29d6a 100644 --- a/MIDAS/src/hardware/Magnetometer.cpp +++ b/MIDAS/src/hardware/Magnetometer.cpp @@ -42,7 +42,7 @@ Magnetometer MagnetometerSensor::read() { // We multiply by 8, which is the full scale of the mag. // https://github.com/sparkfun/SparkFun_MMC5983MA_Magnetometer_Arduino_Library/blob/main/examples/Example4-SPI_Simple_measurement/Example4-SPI_Simple_measurement.ino - Magnetometer reading{X*8, Y*8, Z*8}; + Magnetometer reading{Y*8, -X*8, -Z*8}; return reading; } diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index a3a3e7bb..6af88701 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -286,7 +286,7 @@ DECLARE_THREAD(angularkalman, RocketSystems *arg) AngularKalmanData current_angular_kalman = arg->rocket_data.angular_kalman_data.getRecent(); Acceleration current_accelerations = { - .ax = current_high_g.ax, + .ax = current_high_g.ax; .ay = current_high_g.ay, .az = current_high_g.az}; From 4edef285d7c3c9223090a03301c6ae42fa704086 Mon Sep 17 00:00:00 2001 From: Thomas McManamen Date: Thu, 26 Feb 2026 01:31:32 -0600 Subject: [PATCH 4/7] fixed building issue, attempts to fix bias being 0 --- MIDAS/src/gnc/mqekf.cpp | 10 +++++++--- MIDAS/src/systems.cpp | 2 +- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/MIDAS/src/gnc/mqekf.cpp b/MIDAS/src/gnc/mqekf.cpp index 85c62b02..b4724147 100644 --- a/MIDAS/src/gnc/mqekf.cpp +++ b/MIDAS/src/gnc/mqekf.cpp @@ -14,7 +14,7 @@ void QuaternionMEKF::initialize(RocketSystems *args) { - float Pq0 = 1e-6; + float Pq0 = 1e-4; float Pb0 = 1e-1; sigma_a = {accel_noise_density_x * sqrt(20.0f) * 1.0e-6 * 9.81, accel_noise_density_y * sqrt(20.0f) * 1.0e-6 * 9.81, accel_noise_density_z * sqrt(20.0f) * 1.0e-6 * 9.81}; // ug/sqrt(Hz) *sqrt(hz). values are from datasheet sigma_g = {gyro_RMS_noise * pow(1.0f, -3.0f) * pi / 180.0f * pow(20.0f, 0.5f), gyro_RMS_noise * pow(1.0f, -3.0f) * pi / 180.0f * pow(20.0f, 0.5f), gyro_RMS_noise * pow(1.0f, -3.0f) * pi / 180.0f * pow(20.0f, 0.5f)}; @@ -104,6 +104,10 @@ void QuaternionMEKF::tick(float dt, Magnetometer &magnetometer, Velocity &angula state.gyrobias[0] = bias_gyro(0, 0); state.gyrobias[1] = bias_gyro(1, 0); state.gyrobias[2] = bias_gyro(2, 0); + Serial.print("BIAS 0: "); + Serial.print(state.gyrobias[0]); + Serial.print(" BIAS 1: "); + Serial.println(state.gyrobias[1]); } } @@ -115,7 +119,7 @@ void QuaternionMEKF::time_update(Velocity const &gyro, float Ts) 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 @@ -287,7 +291,7 @@ Eigen::Matrix QuaternionMEKF::initialize_Q(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(); + Q.block<3, 3>(3, 3) = 1e-2 * Eigen::Matrix3f::Identity(); return Q; } diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index 6af88701..a3a3e7bb 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -286,7 +286,7 @@ DECLARE_THREAD(angularkalman, RocketSystems *arg) AngularKalmanData current_angular_kalman = arg->rocket_data.angular_kalman_data.getRecent(); Acceleration current_accelerations = { - .ax = current_high_g.ax; + .ax = current_high_g.ax, .ay = current_high_g.ay, .az = current_high_g.az}; From 2a57a444fd242adec3be1dcb557922f0317db032 Mon Sep 17 00:00:00 2001 From: Divij Date: Fri, 27 Feb 2026 16:34:15 -0500 Subject: [PATCH 5/7] removed serial --- MIDAS/src/gnc/mqekf.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/MIDAS/src/gnc/mqekf.cpp b/MIDAS/src/gnc/mqekf.cpp index b4724147..2077b506 100644 --- a/MIDAS/src/gnc/mqekf.cpp +++ b/MIDAS/src/gnc/mqekf.cpp @@ -104,10 +104,6 @@ void QuaternionMEKF::tick(float dt, Magnetometer &magnetometer, Velocity &angula state.gyrobias[0] = bias_gyro(0, 0); state.gyrobias[1] = bias_gyro(1, 0); state.gyrobias[2] = bias_gyro(2, 0); - Serial.print("BIAS 0: "); - Serial.print(state.gyrobias[0]); - Serial.print(" BIAS 1: "); - Serial.println(state.gyrobias[1]); } } @@ -389,7 +385,6 @@ void QuaternionMEKF::calculate_tilt() state.mq_tilt = tilt; // Serial.print("TILT: "); - Serial.println(tilt * 180.0f / pi); } QuaternionMEKF mqekf; From 677039d8044ca2e32cdd2912c9bb2f08f56962f2 Mon Sep 17 00:00:00 2001 From: Keshav B Date: Fri, 27 Feb 2026 16:41:53 -0600 Subject: [PATCH 6/7] Removing use of BodyToGlobal in ekf --- MIDAS/src/gnc/ekf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index 407ef404..fd457dbd 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -80,8 +80,8 @@ void EKF::priori(float dt, AngularKalmanData &orientation, FSMState fsm, Acceler sensor_accel_global_g(0, 0) = acceleration.ax ; sensor_accel_global_g(1, 0) = acceleration.ay ; sensor_accel_global_g(2, 0) = acceleration.az ; - 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 acceleration 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; From 1ce49bee30020eaef02bfce818f133e0a0f0d568 Mon Sep 17 00:00:00 2001 From: Thomas McManamen Date: Fri, 27 Feb 2026 23:53:57 -0600 Subject: [PATCH 7/7] integrated sflp with mqekf --- MIDAS/src/data_logging.cpp | 2 +- MIDAS/src/gnc/mqekf.cpp | 37 ++++++++++++++++++++++++++++--------- MIDAS/src/gnc/mqekf.h | 5 +++-- MIDAS/src/hardware/Pyro.cpp | 2 +- MIDAS/src/rocket_state.h | 2 +- MIDAS/src/sensor_data.h | 5 +++-- MIDAS/src/systems.cpp | 12 ++++++------ MIDAS/src/telemetry.cpp | 4 ++-- 8 files changed, 45 insertions(+), 24 deletions(-) diff --git a/MIDAS/src/data_logging.cpp b/MIDAS/src/data_logging.cpp index a1fdd771..55d6092f 100644 --- a/MIDAS/src/data_logging.cpp +++ b/MIDAS/src/data_logging.cpp @@ -78,7 +78,7 @@ void log_begin(LogSink& sink) { */ void log_data(LogSink& sink, RocketData& data) { log_from_sensor_data(sink, data.imu); - log_from_sensor_data(sink, data.hw_filtered); + log_from_sensor_data(sink, data.sflp); log_from_sensor_data(sink, data.barometer); log_from_sensor_data(sink, data.voltage); log_from_sensor_data(sink, data.gps); diff --git a/MIDAS/src/gnc/mqekf.cpp b/MIDAS/src/gnc/mqekf.cpp index 2077b506..d6a5d052 100644 --- a/MIDAS/src/gnc/mqekf.cpp +++ b/MIDAS/src/gnc/mqekf.cpp @@ -75,7 +75,7 @@ QuaternionMEKF::QuaternionMEKF() state = AngularKalmanData(); } -void QuaternionMEKF::tick(float dt, Magnetometer &magnetometer, Velocity &angular_velocity, Acceleration &acceleration, FSMState FSM_state) +void QuaternionMEKF::tick(float dt, Magnetometer &magnetometer, Velocity &angular_velocity, Acceleration &acceleration, FSMState FSM_state, Velocity &gyro_bias_sflp) { if (FSM_state >= FSMState::STATE_IDLE) // { @@ -83,9 +83,12 @@ void QuaternionMEKF::tick(float dt, Magnetometer &magnetometer, Velocity &angula // setQ(dt, sd); // priori(dt, orientation, FSM_state, acceleration); // update(barometer, acceleration, orientation, FSM_state, gps); - + x(3) = gyro_bias_sflp.vx * pi/180; + x(4) = gyro_bias_sflp.vy* pi/180; + x(5) = gyro_bias_sflp.vz* pi/180; time_update(angular_velocity, dt); measurement_update(acceleration, magnetometer); + calculate_tilt(); Eigen::Matrix curr_quat = quaternion(); // w,x,y,z @@ -355,11 +358,6 @@ AngularKalmanData QuaternionMEKF::getState() void QuaternionMEKF::calculate_tilt() { - const float alpha = 0.98; // Higher values dampen out current measurements --> reduce peaks - - // The guess & check method! - // Quat --> euler --> rotation matrix --> reference&cur vector --> dot product for angle! - Eigen::Quaternion ref = Eigen::Quaternionf(0, 0, 0, 1); @@ -379,12 +377,33 @@ void QuaternionMEKF::calculate_tilt() } // const float gain = 0.2; - // // Arthur's Comp Filter + // Arthur's Comp Filter // float filtered_tilt = gain * tilt + (1 - gain) * prev_tilt; // prev_tilt = filtered_tilt; state.mq_tilt = tilt; +} + + +void QuaternionMEKF::calculate_tilt(IMU_SFLP imu_sflp) +{ + + Eigen::Vector3f ref(1,0,0); + + Eigen::Quaternion sflp_quat = Eigen::Quaternionf(imu_sflp.quaternion.w,imu_sflp.quaternion.x,imu_sflp.quaternion.y,imu_sflp.quaternion.z); + sflp_quat.normalize(); + + Eigen::Vector3f rotated_vector = sflp_quat * ref; + + Eigen::Vector3f world_vertical(0, 0, 1); + float dot = rotated_vector.dot(world_vertical); + float cur_mag = rotated_vector.norm(); + float tilt = 0; + if (cur_mag != 0) + { + tilt = acos(dot / (cur_mag)); + } - // Serial.print("TILT: "); + state.sflp_tilt = tilt; } QuaternionMEKF mqekf; diff --git a/MIDAS/src/gnc/mqekf.h b/MIDAS/src/gnc/mqekf.h index 6faabe7d..1b5f3bbf 100644 --- a/MIDAS/src/gnc/mqekf.h +++ b/MIDAS/src/gnc/mqekf.h @@ -23,7 +23,7 @@ class QuaternionMEKF Eigen::Ref const> const &vhat, Eigen::Ref const> const &Rm); - void tick(float dt, Magnetometer &magnetometer, Velocity &angular_velocity, Acceleration &acceleration, FSMState FSM_state); + void tick(float dt, Magnetometer &magnetometer, Velocity &angular_velocity, Acceleration &acceleration, FSMState FSM_state, Velocity &gyro_bias_sflp); Eigen::Matrix quaternion(); Eigen::Matrix covariance(); Eigen::Matrix gyroscope_bias(); @@ -32,7 +32,8 @@ class QuaternionMEKF AngularKalmanData getState(); Eigen::Matrix quatToEuler(const Eigen::Matrix &q); // void tick(float dt, float sd, ); - void calculate_tilt(); + void calculate_tilt(); //using internal quaternion + void calculate_tilt(IMU_SFLP imu_sflp); //using SFLP quaternion 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; diff --git a/MIDAS/src/hardware/Pyro.cpp b/MIDAS/src/hardware/Pyro.cpp index 23104737..3aadf7cb 100644 --- a/MIDAS/src/hardware/Pyro.cpp +++ b/MIDAS/src/hardware/Pyro.cpp @@ -26,7 +26,7 @@ bool error_is_failure(GpioError error_code) { */ bool can_fire_igniter(AngularKalmanData angular_kalman_data) { // With new GNC orientation code we can add a simple check. - return angular_kalman_data.comp_tilt < MAXIMUM_TILT_ANGLE; // comp_tilt or mq_tilt? + return angular_kalman_data.sflp_tilt < MAXIMUM_TILT_ANGLE; } diff --git a/MIDAS/src/rocket_state.h b/MIDAS/src/rocket_state.h index ee3f5ca5..efb66523 100644 --- a/MIDAS/src/rocket_state.h +++ b/MIDAS/src/rocket_state.h @@ -189,7 +189,7 @@ struct RocketData { SensorData kalman; SensorData angular_kalman_data; BufferedSensorData imu; - SensorData hw_filtered; + SensorData sflp; BufferedSensorData barometer; SensorData pyro; SensorData fsm_state; diff --git a/MIDAS/src/sensor_data.h b/MIDAS/src/sensor_data.h index 7f0c2386..1f1abd0d 100644 --- a/MIDAS/src/sensor_data.h +++ b/MIDAS/src/sensor_data.h @@ -122,7 +122,7 @@ struct Magnetometer { double mz; }; -struct Quaternion { +struct Quaternion { //long term, remove or rename this struct, it will conflict with libraries where Quaternion is well defined. float w, x, y, z; static float dot(const Quaternion& q1, const Quaternion& q2) { @@ -273,7 +273,8 @@ struct KalmanData { struct AngularKalmanData { Quaternion quaternion; float gyrobias[3]; - double comp_tilt = 0.0; + double sflp_tilt = 0.0; + // double comp_tilt = 0.0; double mq_tilt = 0.0; bool has_data = false; OrientationReadingType reading_type = OrientationReadingType::FULL_READING; diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index a3a3e7bb..037b60a7 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -80,7 +80,7 @@ DECLARE_THREAD(imuthread, RocketSystems *arg) { xSemaphoreTake(spi_mutex, portMAX_DELAY); IMU imudata = arg->sensors.imu.read(); - IMU_SFLP hw_filter = arg->sensors.imu.read_sflp(); + IMU_SFLP sflp = arg->sensors.imu.read_sflp(); xSemaphoreGive(spi_mutex); // Sensor calibration, if it is triggered. @@ -101,7 +101,7 @@ DECLARE_THREAD(imuthread, RocketSystems *arg) imudata.highg_acceleration.az = imudata.highg_acceleration.az + bias.az; arg->rocket_data.imu.update(imudata); - arg->rocket_data.hw_filtered.update(hw_filter); + arg->rocket_data.sflp.update(sflp); THREAD_SLEEP(5); } @@ -276,13 +276,13 @@ DECLARE_THREAD(angularkalman, RocketSystems *arg) TickType_t last = xTaskGetTickCount(); arg->rocket_data.command_flags.should_reset_kf = false; } - + IMU_SFLP current_imu_sflp = arg->rocket_data.sflp.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; Magnetometer current_mag = arg->rocket_data.magnetometer.getRecent(); Velocity current_angular_velocity = current_imu.angular_velocity; // degrees - + Velocity current_gyro_bias = current_imu_sflp.gyro_bias; AngularKalmanData current_angular_kalman = arg->rocket_data.angular_kalman_data.getRecent(); Acceleration current_accelerations = { @@ -294,8 +294,8 @@ DECLARE_THREAD(angularkalman, RocketSystems *arg) float timestamp = pdTICKS_TO_MS(xTaskGetTickCount()) / 1000.0f; // Check with Divij - mqekf.tick(dt, current_mag, current_angular_velocity,current_accelerations, FSM_state); - + mqekf.tick(dt, current_mag, current_angular_velocity, current_accelerations, FSM_state, current_gyro_bias); + mqekf.calculate_tilt(current_imu_sflp); AngularKalmanData current_state = mqekf.getState(); arg->rocket_data.angular_kalman_data.update(current_state); diff --git a/MIDAS/src/telemetry.cpp b/MIDAS/src/telemetry.cpp index 387a8bdb..1b36f6af 100644 --- a/MIDAS/src/telemetry.cpp +++ b/MIDAS/src/telemetry.cpp @@ -83,7 +83,7 @@ TelemetryPacket Telemetry::makePacket(RocketData& data) { TelemetryPacket packet { }; IMU imu = data.imu.getRecentUnsync(); - IMU_SFLP hw_filtered_data = data.hw_filtered.getRecentUnsync(); + IMU_SFLP sflp_data = data.sflp.getRecentUnsync(); GPS gps = data.gps.getRecentUnsync(); Voltage voltage = data.voltage.getRecentUnsync(); Barometer barometer = data.barometer.getRecentUnsync(); @@ -111,7 +111,7 @@ TelemetryPacket Telemetry::makePacket(RocketData& data) { // Tilt & FSM State --> comp_tilt vs mq_tilt static_assert(FSMState::FSM_STATE_COUNT < 16); - uint16_t tilt_norm = (angular_kalman.mq_tilt / M_PI) * 0x0fff; // Encodes tilt value 0-1 into range 0x0000 - 0x0fff + uint16_t tilt_norm = (angular_kalman.sflp_tilt / M_PI) * 0x0fff; // Encodes tilt value 0-1 into range 0x0000 - 0x0fff packet.tilt_fsm |= ((tilt_norm << 4) & 0xfff0); packet.tilt_fsm |= ((uint16_t)fsm & 0x000f);