Skip to content

Commit 6a87fb9

Browse files
mpkarpov-uimpkarpov-ui
authored
Merge pull request #96 from ISSUIUC/16-ekf-repair
fixed gravity issues and splined aerodynamic coeff data into a function
2 parents 7f7336d + 0e905f4 commit 6a87fb9

File tree

3 files changed

+56
-112
lines changed

3 files changed

+56
-112
lines changed

‎MIDAS/src/gnc/ekf.cpp‎

Lines changed: 55 additions & 67 deletions
Original file line numberDiff line numberDiff line change
@@ -26,33 +26,33 @@ typedef struct
2626
} AeroCoeff;
2727

2828
// stores the aerodynamic coefficients for the corresponding Mach number
29-
const AeroCoeff aero_data[] = {
30-
{0.04, 4, 1.332142905, 1.1827808455, 60.8267871968},
31-
{0.08, 4, 1.326587387, 1.1827808455, 60.8267871968},
32-
{0.12, 4, 1.31558627, 1.1827808455, 60.8267871968},
33-
{0.16, 4, 1.306063953, 1.1827808455, 60.8267871968},
34-
{0.20, 4, 1.298117084, 1.1827808455, 60.8267871968},
35-
{0.24, 4, 1.291411025, 1.1827808455, 60.8267871968},
36-
{0.28, 4, 1.290279857, 1.1827808455, 60.8267871968},
37-
{0.32, 4, 1.291431043, 1.1827808455, 60.8267871968},
38-
{0.36, 4, 1.293170653, 1.1827808455, 60.8267871968},
39-
{0.40, 4, 1.295385827, 1.1827808455, 60.8267871968},
40-
{0.44, 4, 1.297991738, 1.1827808455, 60.8267871968},
41-
{0.48, 4, 1.300924032, 1.1827808455, 60.8267871968},
42-
{0.52, 4, 1.304132086, 1.1827808455, 60.8267871968},
43-
{0.56, 4, 1.309039395, 1.1827808455, 60.8267871968},
44-
{0.60, 4, 1.314605487, 1.1827808455, 60.8267871968},
45-
{0.64, 4, 1.330699437, 1.1827808455, 60.8267871968},
46-
{0.68, 4, 1.346695167, 1.1827808455, 60.8267871968},
47-
{0.72, 4, 1.362693183, 1.1827808455, 60.8267871968},
48-
{0.76, 4, 1.378693074, 1.1827808455, 60.8267871968},
49-
{0.80, 4, 1.394695194, 1.1827808455, 60.8267871968},
50-
{0.84, 4, 1.41069913, 1.1827808455, 60.8267871968},
51-
{0.88, 4, 1.426705046, 1.1827808455, 60.8267871968},
52-
{0.92, 4, 1.473732816, 1.218959468, 60.91848997},
53-
{0.96, 4, 1.582395672, 1.291316713, 61.10189551},
54-
{1.00, 4, 1.681494886, 1.363673958, 61.28530105},
55-
};
29+
// const AeroCoeff aero_data[] = {
30+
// {0.04, 4, 1.332142905, 1.1827808455, 60.8267871968},
31+
// {0.08, 4, 1.326587387, 1.1827808455, 60.8267871968},
32+
// {0.12, 4, 1.31558627, 1.1827808455, 60.8267871968},
33+
// {0.16, 4, 1.306063953, 1.1827808455, 60.8267871968},
34+
// {0.20, 4, 1.298117084, 1.1827808455, 60.8267871968},
35+
// {0.24, 4, 1.291411025, 1.1827808455, 60.8267871968},
36+
// {0.28, 4, 1.290279857, 1.1827808455, 60.8267871968},
37+
// {0.32, 4, 1.291431043, 1.1827808455, 60.8267871968},
38+
// {0.36, 4, 1.293170653, 1.1827808455, 60.8267871968},
39+
// {0.40, 4, 1.295385827, 1.1827808455, 60.8267871968},
40+
// {0.44, 4, 1.297991738, 1.1827808455, 60.8267871968},
41+
// {0.48, 4, 1.300924032, 1.1827808455, 60.8267871968},
42+
// {0.52, 4, 1.304132086, 1.1827808455, 60.8267871968},
43+
// {0.56, 4, 1.309039395, 1.1827808455, 60.8267871968},
44+
// {0.60, 4, 1.314605487, 1.1827808455, 60.8267871968},
45+
// {0.64, 4, 1.330699437, 1.1827808455, 60.8267871968},
46+
// {0.68, 4, 1.346695167, 1.1827808455, 60.8267871968},
47+
// {0.72, 4, 1.362693183, 1.1827808455, 60.8267871968},
48+
// {0.76, 4, 1.378693074, 1.1827808455, 60.8267871968},
49+
// {0.80, 4, 1.394695194, 1.1827808455, 60.8267871968},
50+
// {0.84, 4, 1.41069913, 1.1827808455, 60.8267871968},
51+
// {0.88, 4, 1.426705046, 1.1827808455, 60.8267871968},
52+
// {0.92, 4, 1.473732816, 1.218959468, 60.91848997},
53+
// {0.96, 4, 1.582395672, 1.291316713, 61.10189551},
54+
// {1.00, 4, 1.681494886, 1.363673958, 61.28530105},
55+
// };
5656

5757
// Number of entries
5858
#define AERO_DATA_SIZE (sizeof(aero_data) / sizeof(aero_data[0]))
@@ -241,14 +241,22 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm)
241241
Velocity omega = orientation.getVelocity();
242242
euler_t angles = orientation.getEuler();
243243
// Eigen::Matrix<float, 3, 1> gravity = Eigen::Matrix<float, 3,1>::Zero();
244-
gravity(0, 0) = -9.81;
244+
if ((fsm > FSMState::STATE_IDLE) && (fsm < FSMState::STATE_LANDED))
245+
{
246+
gravity(0, 0) = -9.81;
247+
} else {
248+
gravity(0, 0) = 0;
249+
}
250+
245251
float m = mass_sustainer;
246252
float h = height_sustainer;
253+
247254
if (fsm < FSMState::STATE_BURNOUT)
248255
{
249256
m = mass_full;
250257
h = height_full;
251258
}
259+
252260
float w_x = omega.vx;
253261
float w_y = omega.vy;
254262
float w_z = omega.vz;
@@ -264,11 +272,11 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm)
264272

265273
Eigen::Matrix<float, 3, 1> Fg_body;
266274

267-
EKF::GlobalToBody(angles,Fg_body);
275+
EKF::GlobalToBody(angles, gravity);
268276

269-
float Fgx = Fg_body(0, 0);
270-
float Fgy = Fg_body(1, 0);
271-
float Fgz = Fg_body(2, 0);
277+
float Fgx = gravity(0, 0);
278+
float Fgy = gravity(1, 0);
279+
float Fgz = gravity(2, 0);
272280

273281

274282
Eigen::Matrix<float, 3, 1> T;
@@ -293,7 +301,7 @@ void EKF::priori(float dt, Orientation &orientation, FSMState fsm)
293301
(Faz + Ftz + Fgz) / m - (w_x * x_k(4, 0) - w_y * x_k(1, 0)),
294302
1.0;
295303
x_priori = (xdot * dt) + x_k;
296-
setF(dt, fsm, w_x, w_y, w_z);
304+
setF(dt, w_x, w_y, w_z);
297305
P_priori = (F_mat * P_k * F_mat.transpose()) + Q;
298306
}
299307

@@ -319,7 +327,7 @@ float EKF::linearInterpolation(float x0, float y0, float x1, float y1, float x)
319327
* which thrust curve to use. The time since ignition is also important to consider so that is reset once we reach a new stage.
320328
* The thrust force is then rotated into the body frame using the BodyToGlobal function.
321329
*/
322-
void EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state,Eigen::Matrix<float, 3, 1> &to_modify)
330+
void EKF::getThrust(float timestamp, euler_t angles, FSMState FSM_state, Eigen::Matrix<float, 3, 1> &to_modify)
323331
{
324332
float interpolatedValue = 0;
325333
if (FSM_state >= STATE_FIRST_BOOST)
@@ -408,7 +416,7 @@ void EKF::update(Barometer barometer, Acceleration acceleration, Orientation ori
408416
angles.yaw = -angles.yaw;
409417

410418
Eigen::Matrix<float, 3, 1> acc;
411-
EKF::BodyToGlobal(angles, accel,acc);
419+
EKF::BodyToGlobal(angles, accel, acc);
412420

413421
y_k(1, 0) = ((acc)(0)) * 9.81 - 9.81;
414422
y_k(2, 0) = ((acc)(1)) * 9.81;
@@ -457,7 +465,7 @@ void EKF::tick(float dt, float sd, Barometer &barometer, Acceleration accelerati
457465
last_fsm = FSM_state;
458466
}
459467
stage_timestamp += dt;
460-
setF(dt, FSM_state, orientation.roll, orientation.pitch, orientation.yaw);
468+
setF(dt, orientation.roll, orientation.pitch, orientation.yaw);
461469
setQ(dt, sd);
462470
priori(dt, orientation, FSM_state);
463471
update(barometer, acceleration, orientation, FSM_state);
@@ -501,7 +509,8 @@ void EKF::setState(KalmanState state)
501509
* The Q matrix is the covariance matrix for the process noise and is
502510
* updated based on the time taken per cycle of the Kalman Filter Thread.
503511
*/
504-
void EKF::setQ(float dt, float sd)
512+
void EKF::
513+
setQ(float dt, float sd)
505514
{
506515
Q(0, 0) = pow(dt, 5) / 20;
507516
Q(0, 1) = pow(dt, 4) / 8;
@@ -580,6 +589,7 @@ void EKF::GlobalToBody(euler_t angles, Eigen::Matrix<float,3,1> &to_modify)
580589
yaw << cos(angles.yaw), -sin(angles.yaw), 0, sin(angles.yaw), cos(angles.yaw), 0, 0, 0, 1;
581590
Eigen::Matrix3f rotation_matrix = yaw * pitch * roll;
582591
to_modify = rotation_matrix.transpose() * gravity;
592+
583593
// return to_return;
584594
}
585595

@@ -592,7 +602,7 @@ void EKF::GlobalToBody(euler_t angles, Eigen::Matrix<float,3,1> &to_modify)
592602
* by how the states change over time and also depends on the
593603
* current state of the rocket.
594604
*/
595-
void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz)
605+
void EKF::setF(float dt, float wx, float wy, float wz)
596606
{
597607
Eigen::Matrix<float, 3, 1> w = Eigen::Matrix<float, 3, 1>::Zero();
598608
w(0, 0) = wx;
@@ -601,35 +611,13 @@ void EKF::setF(float dt, FSMState fsm, float wx, float wy, float wz)
601611
F_mat(0, 1) = 1;
602612
F_mat(3, 4) = 1;
603613
F_mat(6, 7) = 1;
604-
605-
float velocity_magnitude = pow(x_k(1, 0) * x_k(1, 0) + x_k(4, 0) * x_k(4, 0) + x_k(7, 0) * x_k(7, 0), 0.5);
606-
float mach = velocity_magnitude / a;
607-
int index = std::round(mach / 0.04);
608-
index = std::clamp(index, 0, (int)AERO_DATA_SIZE - 1);
609-
610-
Ca = aero_data[index].CA_power_on;
611-
Cn = aero_data[index].CN;
612-
Cp = aero_data[index].CP;
613-
614-
float m = mass_sustainer;
615-
float h = height_sustainer;
616-
if (fsm < FSMState::STATE_BURNOUT)
617-
{
618-
m = mass_full;
619-
h = height_full;
620-
}
621-
622-
F_mat(1, 1) = -pi * Ca * r * r * rho * x_k(1, 0) / m;
623-
F_mat(1, 4) = -pi * Ca * r * r * rho * x_k(4, 0) / m + w(2, 0);
624-
F_mat(1, 7) = -pi * Ca * r * r * rho * x_k(7, 0) / m - w(1, 0);
625-
626-
F_mat(4, 1) = pi * Cn * r * r * rho * x_k(1, 0) / m - w(2, 0);
627-
F_mat(4, 4) = pi * Cn * r * r * rho * x_k(2, 0) / m + w(0, 0);
628-
F_mat(4, 7) = pi * Cn * r * r * rho * x_k(3, 0) / m;
629-
630-
F_mat(7, 1) = pi * Cn * r * r * rho * x_k(1, 0) / m + w(1, 0);
631-
F_mat(7, 4) = pi * Cn * r * r * rho * x_k(2, 0) / m - w(0, 0);
632-
F_mat(7, 7) = pi * Cn * r * r * rho * x_k(3, 0) / m;
614+
615+
F_mat(1, 4) = w(2, 0);
616+
F_mat(1, 7) = -w(1, 0);
617+
F_mat(4, 1) = -w(2, 0);
618+
F_mat(4, 7) = w(0, 0);
619+
F_mat(7, 1) = w(1, 0);
620+
F_mat(7, 4) = -w(0, 0);
633621
}
634622

635623
EKF ekf;

‎MIDAS/src/gnc/ekf.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@ class EKF : public KalmanFilter<NUM_STATES, NUM_SENSOR_INPUTS>
1818
void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState state) override;
1919

2020
void setQ(float dt, float sd);
21-
void setF(float dt, FSMState fsm, float w_x, float w_y, float w_z);
21+
void setF(float dt, float w_x, float w_y, float w_z);
2222
void getThrust(float timestamp, euler_t angles, FSMState FSM_state, Eigen::Matrix<float, 3, 1> &to_modify);
2323
void BodyToGlobal(euler_t angles, Eigen::Matrix<float, 3, 1> &x_k, Eigen::Matrix<float, 3, 1> &to_modify);
2424
void GlobalToBody(euler_t angles, Eigen::Matrix<float, 3, 1> &to_modify);

‎MIDAS/src/systems.cpp‎

Lines changed: 0 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,7 @@
11
#include "systems.h"
22

33
#include "hal.h"
4-
5-
#ifdef IS_SUSTAINER
64
#include "gnc/ekf.h"
7-
#endif
8-
9-
#ifdef IS_BOOSTER
10-
#include "gnc/yessir.h"
11-
#endif
125

136
#include <TCAL9539.h>
147

@@ -200,7 +193,6 @@ DECLARE_THREAD(buzzer, RocketSystems* arg) {
200193
}
201194
}
202195

203-
#ifdef IS_SUSTAINER
204196
DECLARE_THREAD(kalman, RocketSystems* arg) {
205197
ekf.initialize(arg);
206198
// Serial.println("Initialized ekf :(");
@@ -234,43 +226,7 @@ DECLARE_THREAD(kalman, RocketSystems* arg) {
234226
THREAD_SLEEP(50);
235227
}
236228
}
237-
#endif
238-
239-
#ifdef IS_BOOSTER
240-
DECLARE_THREAD(kalman, RocketSystems* arg) {
241-
yessir.initialize(arg);
242-
Serial.println("Initialized YESSIR");
243-
TickType_t last = xTaskGetTickCount();
244-
245-
while (true) {
246-
if(arg->rocket_data.command_flags.should_reset_kf){
247-
yessir.initialize(arg);
248-
TickType_t last = xTaskGetTickCount();
249-
arg->rocket_data.command_flags.should_reset_kf = false;
250-
}
251-
// add the tick update function
252-
Barometer current_barom_buf = arg->rocket_data.barometer.getRecent();
253-
Orientation current_orientation = arg->rocket_data.orientation.getRecent();
254-
HighGData current_accelerometer = arg->rocket_data.high_g.getRecent();
255-
FSMState FSM_state = arg->rocket_data.fsm_state.getRecent();
256-
Acceleration current_accelerations = {
257-
.ax = current_accelerometer.ax,
258-
.ay = current_accelerometer.ay,
259-
.az = current_accelerometer.az
260-
};
261-
float dt = pdTICKS_TO_MS(xTaskGetTickCount() - last) / 1000.0f;
262-
float timestamp = pdTICKS_TO_MS(xTaskGetTickCount()) / 1000.0f;
263-
yessir.tick(dt, 13.0, current_barom_buf, current_accelerations, current_orientation, FSM_state);
264-
KalmanData current_state = yessir.getState();
265229

266-
arg->rocket_data.kalman.update(current_state);
267-
268-
last = xTaskGetTickCount();
269-
270-
THREAD_SLEEP(50);
271-
}
272-
}
273-
#endif
274230

275231
void handle_tlm_command(TelemetryCommand& command, RocketSystems* arg, FSMState current_state) {
276232
// maybe we should move this somewhere else but it can stay here for now

0 commit comments

Comments
 (0)