Skip to content

Commit f2bb911

Browse files
tjmcmanamen38tjmcmanamen38
authored
Merge pull request #203 from ISSUIUC/check/gnc-feb-launch-final
GNC Commits for MIDAS MINI
2 parents 566073b + 0b3886a commit f2bb911

File tree

12 files changed

+78
-77
lines changed

12 files changed

+78
-77
lines changed

‎MIDAS/src/data_logging.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -78,7 +78,7 @@ void log_begin(LogSink& sink) {
7878
*/
7979
void log_data(LogSink& sink, RocketData& data) {
8080
log_from_sensor_data(sink, data.imu);
81-
log_from_sensor_data(sink, data.hw_filtered);
81+
log_from_sensor_data(sink, data.sflp);
8282
log_from_sensor_data(sink, data.barometer);
8383
log_from_sensor_data(sink, data.voltage);
8484
log_from_sensor_data(sink, data.gps);

‎MIDAS/src/gnc/constants.h‎

Lines changed: 2 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -2,21 +2,9 @@
22

33
// constants
44
const float pi = 3.14159268;
5-
const float a = 343.0; // (m/s) speed of sound
6-
const float rho = 1.225; // average air density
7-
const float r = 0.0396; // (m)
8-
const float height_full = 3.0259; // (m) height of rocket Full Stage
9-
const float height_sustainer = 1.5021; // (m) height of rocket Sustainer
10-
const float mass_sustainer_dry = 3.61; // (kg) Sustainer Dry Mass
11-
const float mass_sustainer_wet = 4.68; // (kg) Sustainer Wet Mass
12-
const float mass_booster_dry = 3.76; // (kg) Booster dry mass
13-
const float mass_booster_wet = 5.91; // (kg) Booster wet mass
14-
const float mass_full = mass_sustainer_wet+mass_booster_wet; // (kg) Total mass wet
15-
const float mass_first_burnout = mass_booster_dry+mass_sustainer_wet;// (kg) Total mass after first stage burnout
16-
const float mass_second_burnout = mass_booster_dry+mass_sustainer_dry;// (kg) Total mass after first stage burnout
175
const float gravity_ms2 = 9.81; // (m/s^2) accel due to gravity
186
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
197
const float accel_noise_density_y = 1000; // ug/sqrt(hz) from the accelerometer on MIDAS MINI
208
const float accel_noise_density_z = 1000; // ug/sqrt(hz) from the accelerometer on MIDAS MINI
21-
const float gyro_RMS_noise = 0.1; // Need to get this data from the datasheet
22-
const float mag_RMS_noise = 0; //mG
9+
const float gyro_RMS_noise =3.8; //mdps/√Hz
10+
const float mag_noise = 0.4; //mG

‎MIDAS/src/gnc/ekf.cpp‎

Lines changed: 13 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -57,8 +57,8 @@ void EKF::initialize(RocketSystems *args)
5757
P_k.block<2, 2>(4, 4) = Eigen::Matrix2f::Identity() * 1e-2f; // z block (pos,vel)
5858

5959
// set Measurement Noise Matrix
60-
R(0, 0) = 1.9; // barometer noise
61-
R(1, 1) = 4.0f; // GPS altitude noise (lower trust)
60+
R(0, 0) = 0.1; // barometer noise
61+
R(1, 1) = 1.5f; // GPS altitude noise (lower trust)
6262
R(2, 2) = 3.0f; // GPS east noise
6363
R(3, 3) = 3.0f; // GPS north noise
6464
}
@@ -77,11 +77,11 @@ void EKF::priori(float dt, AngularKalmanData &orientation, FSMState fsm, Acceler
7777
setB(dt);
7878
// Compute control input at current time step
7979
Eigen::Matrix<float, 3, 1> sensor_accel_global_g = Eigen::Matrix<float, 3, 1>::Zero();
80-
sensor_accel_global_g(0, 0) = acceleration.ax + 0.045f;
81-
sensor_accel_global_g(1, 0) = acceleration.ay - 0.065f;
82-
sensor_accel_global_g(2, 0) = acceleration.az - 0.06f;
83-
euler_t angles_rad = orientation.getEuler();
84-
BodyToGlobal(angles_rad, sensor_accel_global_g);
80+
sensor_accel_global_g(0, 0) = acceleration.ax ;
81+
sensor_accel_global_g(1, 0) = acceleration.ay ;
82+
sensor_accel_global_g(2, 0) = acceleration.az ;
83+
// euler_t angles_rad = orientation.getEuler();
84+
// BodyToGlobal(angles_rad, sensor_accel_global_g);
8585
// Do not apply acceleration if on pad
8686
float g_ms2 = (fsm > FSMState::STATE_IDLE) ? gravity_ms2 : 0.0f;
8787
u_control(0, 0) = sensor_accel_global_g(0, 0) * g_ms2;
@@ -192,20 +192,12 @@ void EKF::update(Barometer barometer, Acceleration acceleration, AngularKalmanDa
192192
landed_state_duration = s_dt; // Reset timer
193193
}
194194

195-
// Only reset velocities if we've been landed for at least 0.5 seconds
196-
if (landed_state_duration >= 0.5f)
197-
{
198-
x_k(3, 0) = 0.0f; // velocity y (vy) = 0 when landed
199-
x_k(5, 0) = 0.0f; // velocity z (vz) = 0 when landed
200-
}
201-
}
202-
else
203-
{
204-
// Negate velocity z if it's negative (so negative becomes positive)
205-
if (x_k(5, 0) < 0) // velocity z (vz)
206-
{
207-
x_k(5, 0) = -x_k(5, 0); // Negate negative velocity z to make it positive
208-
}
195+
// // Only reset velocities if we've been landed for at least 0.5 seconds
196+
// if (landed_state_duration >= 0.5f)
197+
// {
198+
// x_k(3, 0) = 0.0f; // velocity y (vy) = 0 when landed
199+
// x_k(5, 0) = 0.0f; // velocity z (vz) = 0 when landed
200+
// }
209201
}
210202

211203
// Update output state structure

‎MIDAS/src/gnc/ekf.h‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,6 @@ class EKF : public KalmanFilter<NUM_STATES, NUM_SENSOR_INPUTS>
4747
float Cn = 0;
4848
float Wind_alpha = 0.85f;
4949
float Cp = 0;
50-
float curr_mass_kg = mass_full; //(kg) Sustainer + Booster, but value changes over time.
5150
std::vector<float> starting_gps; // latitude, longitude, altitude
5251
std::vector<float> starting_ecef; // x, y, z
5352

‎MIDAS/src/gnc/mqekf.cpp‎

Lines changed: 46 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
#include "mqekf.h"
2-
2+
#include "constants.h"
3+
#include <cmath>
34
/*
45
mag
56
y -> -x
@@ -13,11 +14,11 @@
1314

1415
void QuaternionMEKF::initialize(RocketSystems *args)
1516
{
16-
float Pq0 = 1e-6;
17+
float Pq0 = 1e-4;
1718
float Pb0 = 1e-1;
18-
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
19-
sigma_g = {0.1 * pi / 180, 0.1 * pi / 180, 0.1 * pi / 180}; // 0.1 deg/s
20-
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
19+
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
20+
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)};
21+
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
2122
Q = initialize_Q(sigma_g);
2223
Eigen::Matrix<float, 6, 1> sigmas;
2324
sigmas << sigma_a, sigma_m;
@@ -74,17 +75,21 @@ QuaternionMEKF::QuaternionMEKF()
7475
state = AngularKalmanData();
7576
}
7677

77-
void QuaternionMEKF::tick(float dt, Magnetometer &magnetometer, Velocity &angular_velocity, Acceleration &acceleration, FSMState FSM_state)
78+
void QuaternionMEKF::tick(float dt, Magnetometer &magnetometer, Velocity &angular_velocity, Acceleration &acceleration, FSMState FSM_state, Velocity &gyro_bias_sflp)
7879
{
7980
if (FSM_state >= FSMState::STATE_IDLE) //
8081
{
8182

8283
// setQ(dt, sd);
8384
// priori(dt, orientation, FSM_state, acceleration);
8485
// update(barometer, acceleration, orientation, FSM_state, gps);
85-
86+
x(3) = gyro_bias_sflp.vx * pi/180;
87+
x(4) = gyro_bias_sflp.vy* pi/180;
88+
x(5) = gyro_bias_sflp.vz* pi/180;
8689
time_update(angular_velocity, dt);
8790
measurement_update(acceleration, magnetometer);
91+
92+
calculate_tilt();
8893
Eigen::Matrix<float, 4, 1> curr_quat = quaternion(); // w,x,y,z
8994

9095
state.quaternion.w = curr_quat(0, 0);
@@ -113,7 +118,7 @@ void QuaternionMEKF::time_update(Velocity const &gyro, float Ts)
113118
gyr(0, 0) = gyro.vx * (pi / 180.0f);
114119
gyr(1, 0) = gyro.vy * (pi / 180.0f);
115120
gyr(2, 0) = gyro.vz * (pi / 180.0f);
116-
121+
117122
set_transition_matrix(gyr - x.tail(3), Ts);
118123

119124
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,
285290
{
286291
Eigen::Matrix<float, 6, 6> Q = Eigen::Matrix<float, 6, 6>::Zero();
287292
Q.block<3, 3>(0, 0) = sigma_g.array().square().matrix().asDiagonal();
288-
Q.block<3, 3>(3, 3) = 1e-12 * Eigen::Matrix3f::Identity();
293+
Q.block<3, 3>(3, 3) = 1e-2 * Eigen::Matrix3f::Identity();
289294
return Q;
290295
}
291296

@@ -301,9 +306,9 @@ void QuaternionMEKF::initialize_from_acc_mag(Acceleration const &acc_struct, Mag
301306
Eigen::Matrix<float, 3, 1> const acc_normalized = acc / anorm;
302307
Eigen::Matrix<float, 3, 1> const mag_normalized = mag.normalized();
303308

304-
Eigen::Matrix<float, 3, 1> const Rz = -acc_normalized;
305-
Eigen::Matrix<float, 3, 1> const Ry = Rz.cross(mag_normalized).normalized();
306-
Eigen::Matrix<float, 3, 1> const Rx = Ry.cross(Rz).normalized();
309+
Eigen::Matrix<float, 3, 1> const Rx = acc_normalized;
310+
Eigen::Matrix<float, 3, 1> const Rz = Rx.cross(mag_normalized).normalized();
311+
Eigen::Matrix<float, 3, 1> const Ry = Rz.cross(Rx).normalized();
307312

308313
// Construct the rotation matrix
309314
Eigen::Matrix<float, 3, 3> const R = (Eigen::Matrix<float, 3, 3>() << Rx, Ry, Rz).finished();
@@ -353,17 +358,12 @@ AngularKalmanData QuaternionMEKF::getState()
353358
void QuaternionMEKF::calculate_tilt()
354359
{
355360

356-
const float alpha = 0.98; // Higher values dampen out current measurements --> reduce peaks
357-
358-
// The guess & check method!
359-
// Quat --> euler --> rotation matrix --> reference&cur vector --> dot product for angle!
360-
361361
Eigen::Quaternion<float>
362-
ref = Eigen::Quaternionf(1, 0, 0, 0);
362+
ref = Eigen::Quaternionf(0, 0, 0, 1);
363363

364364
Eigen::Quaternion<float> rotated = qref * ref * qref.conjugate();
365365

366-
Eigen::Matrix<float, 1, 3> reference_vector = {0, 0, -1};
366+
Eigen::Matrix<float, 1, 3> reference_vector = {0, 0, 1};
367367
Eigen::Matrix<float, 1, 3> rotated_vector = {rotated.x(), rotated.y(), rotated.z()};
368368

369369
float dot = rotated_vector.dot(reference_vector);
@@ -376,14 +376,34 @@ void QuaternionMEKF::calculate_tilt()
376376
tilt = acos(dot / (cur_mag * ref_mag));
377377
}
378378

379-
const float gain = 0.2;
379+
// const float gain = 0.2;
380380
// Arthur's Comp Filter
381-
float filtered_tilt = gain * tilt + (1 - gain) * prev_tilt;
382-
prev_tilt = filtered_tilt;
383-
state.mq_tilt = filtered_tilt;
381+
// float filtered_tilt = gain * tilt + (1 - gain) * prev_tilt;
382+
// prev_tilt = filtered_tilt;
383+
state.mq_tilt = tilt;
384+
}
385+
386+
387+
void QuaternionMEKF::calculate_tilt(IMU_SFLP imu_sflp)
388+
{
389+
390+
Eigen::Vector3f ref(1,0,0);
391+
392+
Eigen::Quaternion<float> sflp_quat = Eigen::Quaternionf(imu_sflp.quaternion.w,imu_sflp.quaternion.x,imu_sflp.quaternion.y,imu_sflp.quaternion.z);
393+
sflp_quat.normalize();
394+
395+
Eigen::Vector3f rotated_vector = sflp_quat * ref;
396+
397+
Eigen::Vector3f world_vertical(0, 0, 1);
398+
float dot = rotated_vector.dot(world_vertical);
399+
float cur_mag = rotated_vector.norm();
400+
float tilt = 0;
401+
if (cur_mag != 0)
402+
{
403+
tilt = acos(dot / (cur_mag));
404+
}
384405

385-
// Serial.print("TILT: ");
386-
// Serial.println(filtered_tilt * (180/3.14f));
406+
state.sflp_tilt = tilt;
387407
}
388408

389-
QuaternionMEKF mqekf;
409+
QuaternionMEKF mqekf;

‎MIDAS/src/gnc/mqekf.h‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@ class QuaternionMEKF
2323
Eigen::Ref<Eigen::Matrix<float, 3, 1> const> const &vhat,
2424
Eigen::Ref<Eigen::Matrix<float, 3, 3> const> const &Rm);
2525

26-
void tick(float dt, Magnetometer &magnetometer, Velocity &angular_velocity, Acceleration &acceleration, FSMState FSM_state);
26+
void tick(float dt, Magnetometer &magnetometer, Velocity &angular_velocity, Acceleration &acceleration, FSMState FSM_state, Velocity &gyro_bias_sflp);
2727
Eigen::Matrix<float, 4, 1> quaternion();
2828
Eigen::Matrix<float, 6, 6> covariance();
2929
Eigen::Matrix<float, 3, 1> gyroscope_bias();
@@ -32,7 +32,8 @@ class QuaternionMEKF
3232
AngularKalmanData getState();
3333
Eigen::Matrix<float, 3, 1> quatToEuler(const Eigen::Matrix<float, 4, 1> &q);
3434
// void tick(float dt, float sd, );
35-
void calculate_tilt();
35+
void calculate_tilt(); //using internal quaternion
36+
void calculate_tilt(IMU_SFLP imu_sflp); //using SFLP quaternion
3637
void set_transition_matrix(const Eigen::Ref<const Eigen::Matrix<float, 3, 1>> &gyr, float Ts);
3738
Eigen::Matrix<float, 3, 3> skew_symmetric_matrix(const Eigen::Ref<const Eigen::Matrix<float, 3, 1>> &vec) const;
3839
Eigen::Matrix<float, 3, 1> magnetometer_measurement_func() const;

‎MIDAS/src/hardware/Magnetometer.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ Magnetometer MagnetometerSensor::read() {
4242

4343
// We multiply by 8, which is the full scale of the mag.
4444
// https://github.com/sparkfun/SparkFun_MMC5983MA_Magnetometer_Arduino_Library/blob/main/examples/Example4-SPI_Simple_measurement/Example4-SPI_Simple_measurement.ino
45-
Magnetometer reading{X*8, Y*8, Z*8};
45+
Magnetometer reading{Y*8, -X*8, -Z*8};
4646
return reading;
4747
}
4848

‎MIDAS/src/hardware/Pyro.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ bool error_is_failure(GpioError error_code) {
2626
*/
2727
bool can_fire_igniter(AngularKalmanData angular_kalman_data) {
2828
// With new GNC orientation code we can add a simple check.
29-
return angular_kalman_data.mq_tilt < MAXIMUM_TILT_ANGLE;
29+
return angular_kalman_data.sflp_tilt < MAXIMUM_TILT_ANGLE;
3030
}
3131

3232

‎MIDAS/src/rocket_state.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -188,7 +188,7 @@ struct RocketData {
188188
SensorData<KalmanData> kalman;
189189
SensorData<AngularKalmanData> angular_kalman_data;
190190
BufferedSensorData<IMU, 16> imu;
191-
SensorData<IMU_SFLP> hw_filtered;
191+
SensorData<IMU_SFLP> sflp;
192192
BufferedSensorData<Barometer, 16> barometer;
193193
SensorData<PyroState> pyro;
194194
SensorData<FSMState> fsm_state;

‎MIDAS/src/sensor_data.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -122,7 +122,7 @@ struct Magnetometer {
122122
double mz;
123123
};
124124

125-
struct Quaternion {
125+
struct Quaternion { //long term, remove or rename this struct, it will conflict with libraries where Quaternion is well defined.
126126
float w, x, y, z;
127127

128128
static float dot(const Quaternion& q1, const Quaternion& q2) {
@@ -264,6 +264,7 @@ struct KalmanData {
264264
struct AngularKalmanData {
265265
Quaternion quaternion;
266266
float gyrobias[3];
267+
double sflp_tilt = 0.0;
267268
double mq_tilt = 0.0;
268269
bool has_data = false;
269270

0 commit comments

Comments
 (0)