+
+
+
+
+
+
+
+
+
10const float pi = 3.14159268;
+
+
12const float rho = 1.225;
+
13const float r = 0.03935;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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},
+
+
+
+
58#define AERO_DATA_SIZE (sizeof(aero_data) / sizeof(aero_data[0]))
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
149 Orientation orientation =
args->rocket_data.orientation.getRecentUnsync();
+
+
+
152 for (
int i = 0; i < 30; i++)
+
+
154 Barometer barometer =
args->rocket_data.barometer.getRecent();
+
155 LowGData initial_accelerometer =
args->rocket_data.low_g.getRecent();
+
+
157 .
ax = initial_accelerometer.
ax,
+
158 .ay = initial_accelerometer.
ay,
+
159 .az = initial_accelerometer.
az};
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
176 x_k(0, 0) = sum / 30;
+
+
+
+
+
+
182 Q(0, 0) = pow(
s_dt, 5) / 20;
+
183 Q(0, 1) = pow(
s_dt, 4) / 8;
+
184 Q(0, 2) = pow(
s_dt, 3) / 6;
+
185 Q(1, 1) = pow(
s_dt, 3) / 8;
+
186 Q(1, 2) = pow(
s_dt, 2) / 2;
+
+
+
+
+
+
192 Q(3, 3) = pow(
s_dt, 5) / 20;
+
193 Q(3, 4) = pow(
s_dt, 4) / 8;
+
194 Q(3, 5) = pow(
s_dt, 3) / 6;
+
195 Q(4, 4) = pow(
s_dt, 3) / 8;
+
196 Q(4, 5) = pow(
s_dt, 2) / 2;
+
+
+
+
+
+
202 Q(6, 6) = pow(
s_dt, 5) / 20;
+
203 Q(6, 7) = pow(
s_dt, 4) / 8;
+
204 Q(6, 8) = pow(
s_dt, 3) / 6;
+
205 Q(7, 7) = pow(
s_dt, 3) / 8;
+
206 Q(7, 8) = pow(
s_dt, 2) / 2;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
240 Eigen::Matrix<float, 9, 1> xdot = Eigen::Matrix<float, 9, 1>::Zero();
+
+
+
+
+
+
+
+
+
+
+
+
252 float w_x = omega.
vx;
+
253 float w_y = omega.
vy;
+
254 float w_z = omega.
vz;
+
+
256 float J_x = 0.5 * m *
r *
r;
+
257 float J_y = (1 / 3) * m * h * h + 0.25 * m *
r *
r;
+
+
+
260 float vel_mag_squared =
x_k(1, 0) *
x_k(1, 0) +
x_k(4, 0) *
x_k(4, 0) +
x_k(7, 0) *
x_k(7, 0);
+
+
262 float Fax = -0.5 *
rho * (vel_mag_squared) *
float(
Ca) * (
pi *
r *
r);
+
263 float Fay = 0;
float Faz = 0;
+
+
265 Eigen::Matrix<float, 3, 1> Fg_body;
+
+
+
+
269 float Fgx = Fg_body(0, 0);
+
270 float Fgy = Fg_body(1, 0);
+
271 float Fgz = Fg_body(2, 0);
+
+
+
274 Eigen::Matrix<float, 3, 1> T;
+
+
+
+
+
+
+
+
+
+
+
285 (Fax + Ftx + Fgx) / m - (w_y *
x_k(7, 0) - w_z *
x_k(4, 0)),
+
+
+
+
289 (Fay + Fty + Fgy) / m - (w_z *
x_k(1, 0) - w_x *
x_k(7, 0)),
+
+
+
+
293 (Faz + Ftz + Fgz) / m - (w_x *
x_k(4, 0) - w_y *
x_k(1, 0)),
+
+
+
+
+
+
+
+
+
305 return y0 + ((x - x0) * (y1 - y0) / (x1 - x0));
+
+
+
+
+
324 float interpolatedValue = 0;
+
+
+
+
+
+
330 if (timestamp >= 0.009)
+
+
+
+
+
335 float x0 = it->first;
+
336 float y0 = it->second;
+
+
338 float x1 = it->first;
+
339 float y1 = it->second;
+
+
+
+
+
+
+
346 if (timestamp >= 0.083)
+
+
+
+
+
+
352 float x0 = it->first;
+
353 float y0 = it->second;
+
+
355 float x1 = it->first;
+
356 float y1 = it->second;
+
+
+
+
+
+
362 Eigen::Matrix<float, 3, 1> interpolatedVector = Eigen::Matrix<float, 3, 1>::Zero();
+
363 (interpolatedVector)(0, 0) = interpolatedValue;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
395 Eigen::Matrix<float, 4, 4> S_k = Eigen::Matrix<float, 4, 4>::Zero();
+
396 S_k = (((
H *
P_priori *
H.transpose()) +
R)).inverse();
+
397 Eigen::Matrix<float, 9, 9> identity = Eigen::Matrix<float, 9, 9>::Identity();
+
+
+
+
401 Eigen::Matrix<float, 3, 1> accel = Eigen::Matrix<float, 3, 1>(Eigen::Matrix<float, 3, 1>::Zero());
+
+
403 (accel)(0, 0) = acceleration.
az - 0.045;
+
404 (accel)(1, 0) = acceleration.
ay - 0.065;
+
405 (accel)(2, 0) = -acceleration.
ax - 0.06;
+
+
+
+
+
410 Eigen::Matrix<float, 3, 1> acc;
+
+
+
413 y_k(1, 0) = ((acc)(0)) * 9.81 - 9.81;
+
414 y_k(2, 0) = ((acc)(1)) * 9.81;
+
415 y_k(3, 0) = ((acc)(2)) * 9.81;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
462 priori(dt, orientation, FSM_state);
+
463 update(barometer, acceleration, orientation, FSM_state);
+
+
+
+
+
+
+
+
+
+
+
484 this->state.position.px =
state.state_est_pos_x;
+
485 this->state.position.py =
state.state_est_pos_y;
+
486 this->state.position.pz =
state.state_est_pos_z;
+
487 this->state.acceleration.ax =
state.state_est_accel_x;
+
488 this->state.acceleration.ay =
state.state_est_accel_y;
+
489 this->state.acceleration.az =
state.state_est_accel_z;
+
490 this->state.velocity.vx =
state.state_est_vel_x;
+
491 this->state.velocity.vy =
state.state_est_vel_y;
+
492 this->state.velocity.vz =
state.state_est_vel_z;
+
+
+
+
+
506 Q(0, 0) = pow(dt, 5) / 20;
+
507 Q(0, 1) = pow(dt, 4) / 8;
+
508 Q(0, 2) = pow(dt, 3) / 6;
+
509 Q(1, 1) = pow(dt, 3) / 8;
+
510 Q(1, 2) = pow(dt, 2) / 2;
+
+
+
+
+
515 Q(3, 3) = pow(dt, 5) / 20;
+
516 Q(3, 4) = pow(dt, 4) / 8;
+
517 Q(3, 5) = pow(dt, 3) / 6;
+
518 Q(4, 4) = pow(dt, 3) / 8;
+
519 Q(4, 5) = pow(dt, 2) / 2;
+
+
+
+
+
+
525 Q(6, 6) = pow(dt, 5) / 20;
+
526 Q(6, 7) = pow(dt, 4) / 8;
+
527 Q(6, 8) = pow(dt, 3) / 6;
+
528 Q(7, 7) = pow(dt, 3) / 8;
+
529 Q(7, 8) = pow(dt, 2) / 2;
+
+
+
+
+
+
+
+
+
+
+
547 Eigen::Matrix3f roll, pitch, yaw;
+
548 roll << 1., 0., 0., 0., cos(angles.
roll), -sin(angles.
roll), 0., sin(angles.
roll), cos(angles.
roll);
+
549 pitch << cos(angles.
pitch), 0., sin(angles.
pitch), 0., 1., 0., -sin(angles.
pitch), 0., cos(angles.
pitch);
+
550 yaw << cos(angles.
yaw), -sin(angles.
yaw), 0., sin(angles.
yaw), cos(angles.
yaw), 0., 0., 0., 1.;
+
+
552 to_modify = yaw * pitch * roll * (body_vect);
+
+
+
+
+
+
+
+
+
+
+
575 Eigen::Matrix3f roll;
+
576 roll << 1, 0, 0, 0, cos(angles.
roll), -sin(angles.
roll), 0, sin(angles.
roll), cos(angles.
roll);
+
577 Eigen::Matrix3f pitch;
+
578 pitch << cos(angles.
pitch), 0, sin(angles.
pitch), 0, 1, 0, -sin(angles.
pitch), 0, cos(angles.
pitch);
+
+
580 yaw << cos(angles.
yaw), -sin(angles.
yaw), 0, sin(angles.
yaw), cos(angles.
yaw), 0, 0, 0, 1;
+
581 Eigen::Matrix3f rotation_matrix = yaw * pitch * roll;
+
582 to_modify = rotation_matrix.transpose() *
gravity;
+
+
+
+
+
+
597 Eigen::Matrix<float, 3, 1> w = Eigen::Matrix<float, 3, 1>::Zero();
+
+
+
+
+
+
+
+
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);
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
void initialize(RocketSystems *args) override
Sets altitude by averaging 30 barometer measurements taken 100 ms apart.
+
void BodyToGlobal(euler_t angles, Eigen::Matrix< float, 3, 1 > &x_k, Eigen::Matrix< float, 3, 1 > &to_modify)
Converts a vector in the body frame to the global frame.
+
void tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState state)
Run Kalman filter calculations as long as FSM has passed IDLE.
+
+
KalmanData getState() override
Getter for state X.
+
void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState state) override
Update Kalman Gain and state estimate with current sensor data.
+
Eigen::Matrix< float, 3, 1 > gravity
+
Buffer< float, ALTITUDE_BUFFER_SIZE > alt_buffer
+
Eigen::Matrix< float, 3, 1 > init_accel
+
void setQ(float dt, float sd)
Sets the Q matrix given time step and spectral density.
+
void GlobalToBody(euler_t angles, Eigen::Matrix< float, 3, 1 > &to_modify)
Converts a vector in the global frame to the body frame.
+
+
+
+
+
+
float linearInterpolation(float x0, float y0, float x1, float y1, float x)
linearly interpolates the a value based on the lower and upper bound, similar to lerp_() in PySim
+
void setState(KalmanState state) override
Sets state vector x.
+
void getThrust(float timestamp, euler_t angles, FSMState FSM_state, Eigen::Matrix< float, 3, 1 > &to_modify)
Returns the approximate thrust force from the motor given the thurst curve.
+
+
void setF(float dt, FSMState fsm, float w_x, float w_y, float w_z)
Sets the F matrix given time step.
+
+
Eigen::Matrix< float, _NumStates, _NumStates > P_k
+
Eigen::Matrix< float, _NumStates, 1 > x_priori
+
Eigen::Matrix< float, _NumStates, 1 > x_k
+
Eigen::Matrix< float, _NumInputs, _NumInputs > R
+
Eigen::Matrix< float, _NumInputs, 1 > y_k
+
Eigen::Matrix< float, _NumStates, _NumInputs > K
+
Eigen::Matrix< float, _NumStates, _NumInputs > B
+
Eigen::Matrix< float, _NumStates, _NumStates > Q
+
Eigen::Matrix< float, _NumStates, _NumStates > P_priori
+
Eigen::Matrix< float, _NumInputs, _NumStates > H
+
Eigen::Matrix< float, _NumStates, _NumStates > F_mat
+
const float mass_sustainer
+
std::map< float, float > O5500X_data
+
+
const AeroCoeff aero_data[]
+
+
+
+
+
+
+
+
const std::map< float, float > moonburner_data
+
const float height_sustainer
+
+
+
FSMState
Enum for the different FSM states.
+
+
+
+
+
+
#define THREAD_SLEEP(millis)
Delays the running thread.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
data from the Kalman thread
+
Acceleration acceleration
+
+
+
+
+
+
+
+
+
+
+
+
+
Structs starting here represent specific sensors and the respective data.
+
+
+
+
+
Velocity getVelocity() const
+
+
+
+
+
+
+
+
+
+
+
euler representation of rotation
+
+
+
+
1.9.2
+ 
+
+
Public Member Functions inherited from