GNC Commits for MIDAS MINI

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);

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
+const float gyro_RMS_noise =3.8; //mdps/√Hz
+const float mag_noise = 0.4; //mG

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,11 +77,11 @@ void EKF::priori(float dt, AngularKalmanData &orientation, FSMState fsm, Acceler
     setB(dt);
     // Compute control input at current time step
     Eigen::Matrix<float, 3, 1> sensor_accel_global_g = Eigen::Matrix<float, 3, 1>::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);
+    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
     float g_ms2 = (fsm > FSMState::STATE_IDLE) ? gravity_ms2 : 0.0f;
     u_control(0, 0) = sensor_accel_global_g(0, 0) * g_ms2;
@@ -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

MIDAS/src/gnc/ekf.h

@@ -47,7 +47,6 @@ class EKF : public KalmanFilter<NUM_STATES, NUM_SENSOR_INPUTS>
     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<float> starting_gps;    // latitude, longitude, altitude
     std::vector<float> starting_ecef;   // x, y, z
 

MIDAS/src/gnc/mqekf.cpp

@@ -1,5 +1,6 @@
 #include "mqekf.h"
-
+#include "constants.h"
+#include <cmath>
 /*
     mag
     y -> -x
@@ -13,11 +14,11 @@
 
 void QuaternionMEKF::initialize(RocketSystems *args)
 {
-    float Pq0 = 1e-6;
+    float Pq0 = 1e-4;
     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(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<float, 6, 1> sigmas;
     sigmas << sigma_a, sigma_m;
@@ -74,17 +75,21 @@ 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) //
     {
 
         // 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<float, 4, 1> curr_quat = quaternion(); // w,x,y,z
 
         state.quaternion.w = curr_quat(0, 0);
@@ -113,7 +118,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
@@ -285,7 +290,7 @@ Eigen::Matrix<float, 6, 6> QuaternionMEKF::initialize_Q(Eigen::Matrix<float, 3,
 {
     Eigen::Matrix<float, 6, 6> Q = Eigen::Matrix<float, 6, 6>::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;
 }
 
@@ -301,9 +306,9 @@ void QuaternionMEKF::initialize_from_acc_mag(Acceleration const &acc_struct, Mag
     Eigen::Matrix<float, 3, 1> const acc_normalized = acc / anorm;
     Eigen::Matrix<float, 3, 1> const mag_normalized = mag.normalized();
 
-    Eigen::Matrix<float, 3, 1> const Rz = -acc_normalized;
-    Eigen::Matrix<float, 3, 1> const Ry = Rz.cross(mag_normalized).normalized();
-    Eigen::Matrix<float, 3, 1> const Rx = Ry.cross(Rz).normalized();
+    Eigen::Matrix<float, 3, 1> const Rx = acc_normalized;
+    Eigen::Matrix<float, 3, 1> const Rz = Rx.cross(mag_normalized).normalized();
+    Eigen::Matrix<float, 3, 1> const Ry = Rz.cross(Rx).normalized();
 
     // Construct the rotation matrix
     Eigen::Matrix<float, 3, 3> const R = (Eigen::Matrix<float, 3, 3>() << Rx, Ry, Rz).finished();
@@ -353,17 +358,12 @@ 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<float>
-        ref = Eigen::Quaternionf(1, 0, 0, 0);
+        ref = Eigen::Quaternionf(0, 0, 0, 1);
 
     Eigen::Quaternion<float> rotated = qref * ref * qref.conjugate();
 
-    Eigen::Matrix<float, 1, 3> reference_vector = {0, 0, -1};
+    Eigen::Matrix<float, 1, 3> reference_vector = {0, 0, 1};
     Eigen::Matrix<float, 1, 3> rotated_vector = {rotated.x(), rotated.y(), rotated.z()};
 
     float dot = rotated_vector.dot(reference_vector);
@@ -376,14 +376,34 @@ void QuaternionMEKF::calculate_tilt()
         tilt = acos(dot / (cur_mag * ref_mag));
     }
 
-    const float gain = 0.2;
+    // 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;
+    // 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<float> 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: ");
-    // Serial.println(filtered_tilt * (180/3.14f));
+    state.sflp_tilt = tilt;
 }
 
-QuaternionMEKF mqekf;
+QuaternionMEKF mqekf;

MIDAS/src/gnc/mqekf.h

@@ -23,7 +23,7 @@ class QuaternionMEKF
         Eigen::Ref<Eigen::Matrix<float, 3, 1> const> const &vhat,
         Eigen::Ref<Eigen::Matrix<float, 3, 3> 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<float, 4, 1> quaternion();
     Eigen::Matrix<float, 6, 6> covariance();
     Eigen::Matrix<float, 3, 1> gyroscope_bias();
@@ -32,7 +32,8 @@ class QuaternionMEKF
     AngularKalmanData getState();
     Eigen::Matrix<float, 3, 1> quatToEuler(const Eigen::Matrix<float, 4, 1> &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<const Eigen::Matrix<float, 3, 1>> &gyr, float Ts);
     Eigen::Matrix<float, 3, 3> skew_symmetric_matrix(const Eigen::Ref<const Eigen::Matrix<float, 3, 1>> &vec) const;
     Eigen::Matrix<float, 3, 1> magnetometer_measurement_func() const;

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;
 }
 

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.mq_tilt < MAXIMUM_TILT_ANGLE;
+    return angular_kalman_data.sflp_tilt < MAXIMUM_TILT_ANGLE; 
 }
 
 

MIDAS/src/rocket_state.h

@@ -188,7 +188,7 @@ struct RocketData {
     SensorData<KalmanData> kalman;
     SensorData<AngularKalmanData> angular_kalman_data;
     BufferedSensorData<IMU, 16> imu;
-    SensorData<IMU_SFLP> hw_filtered;
+    SensorData<IMU_SFLP> sflp;
     BufferedSensorData<Barometer, 16> barometer;
     SensorData<PyroState> pyro;
     SensorData<FSMState> fsm_state;

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) {
@@ -264,6 +264,7 @@ struct KalmanData {
 struct AngularKalmanData {
     Quaternion quaternion;
     float gyrobias[3];
+    double sflp_tilt = 0.0;
     double mq_tilt = 0.0;
     bool has_data = false;
 

MIDAS/src/systems.cpp

@@ -75,7 +75,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.
@@ -96,7 +96,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);
     }
@@ -271,13 +271,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 = {
@@ -289,8 +289,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);

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
     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);