diff --git a/src/shared/algorithms/MEA/MEA.cpp b/src/shared/algorithms/MEA/MEA.cpp
index eeedf74c0eafe389a45b247a84d0e80da822c8d5..399ee03ee6c06a36c2b8cd91e64af6f83215f895 100644
--- a/src/shared/algorithms/MEA/MEA.cpp
+++ b/src/shared/algorithms/MEA/MEA.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -24,42 +24,170 @@
#include <drivers/timer/TimestampTimer.h>
-namespace Boardcore
+using namespace Boardcore;
+using namespace Eigen;
+
+MEA::Step::Step(float mainValveOpen) : mainValveOpen{mainValveOpen} {}
+
+void MEA::Step::withCCPressure(PressureData ccPressure)
{
+ withCCPressure(ccPressure.pressure);
+}
-MEA::MEA(const KalmanFilter::KalmanConfig kalmanConfig)
- : filter(kalmanConfig), state()
+void MEA::Step::withCCPressure(float ccPressure)
{
- updateState();
+ this->ccPressure = ccPressure;
+ hasCCPressure = true;
}
-void MEA::update(const float feedValvePosition, const float pressure)
+void MEA::Step::withAcceleration(AccelerometerData acceleration)
{
- // Update the Kalman filter
- filter.predictWithControl(KalmanFilter::CVectorM{feedValvePosition});
- filter.correct(KalmanFilter::CVectorP{pressure});
+ withAcceleration(acceleration);
+}
- // Compute the derived values for useful data
+void MEA::Step::withAcceleration(Eigen::Vector<float, 3> acceleration)
+{
+ this->acceleration = acceleration;
+ hasAcceleration = true;
+}
+
+void MEA::Step::withSpeedAndAlt(float verticalSpeed, float mslAltitude)
+{
+ this->verticalSpeed = verticalSpeed;
+ this->mslAltitude = mslAltitude;
+ hasSpeedAndAlt = true;
+}
+
+MEA::MEA(const Config &config)
+ : F{config.F}, Q{config.Q}, G{config.G}, baroH{config.baroH},
+ baroR{config.baroR}, P{config.P}, x{0, 0, config.initialMass},
+ accelThresh{config.accelThresh}, speedThresh{config.speedThresh},
+ Kt{config.Kt}, alpha{config.alpha}, c{config.c}, coeffs{config.coeffs},
+ crossSection{config.crossSection}, ae{config.ae}, p0{config.p0}
+{
+}
+
+void MEA::update(const Step &step)
+{
+ // First run the prediction step
+ predict(step);
+
+ // Compute applied force/mach/CD/rho
+ computeForce(step);
+
+ // Run cc pressure correction
+ correctBaro(step);
+
+ // Run accelerometer correction
+ correctAccel(step);
+
+ // Run apogee prediction
+ computeApogee(step);
+
+ // Finally update state
updateState();
}
MEAState MEA::getState() { return state; }
-void MEA::setKalmanConfig(KalmanFilter::KalmanConfig config)
+void MEA::predict(const Step &step)
{
- filter.setConfig(config);
+ x = F * x + G * step.mainValveOpen;
+ P = F * P * F.transpose() + Q;
}
-void MEA::updateState()
+void MEA::computeForce(const Step &step)
+{
+ if (!step.hasSpeedAndAlt)
+ return;
+
+ // NOTE: Here we assume that verticalSpeed roughly equals the total speed,
+ // so that we don't depend on N/E speed components, which can be quite
+ // unreliable in case of no GPS fix
+ mach = Aeroutils::computeMach(-step.mslAltitude, step.verticalSpeed,
+ Constants::MSL_TEMPERATURE);
+
+ cd = Aeroutils::computeCd(coeffs, mach);
+ rho = Aeroutils::computeRho(-step.mslAltitude, Constants::MSL_TEMPERATURE);
+
+ // Dynamic pressure
+ q = 0.5f * rho * (step.verticalSpeed * step.verticalSpeed);
+
+ // Aerodynamic force component
+ force = q * crossSection * cd;
+
+ if (step.mslAltitude > 800)
+ {
+ // At high altitudes we need to compensate for low external pressure
+
+ // External pressure
+ float p = Aeroutils::relPressure(step.mslAltitude);
+ force -= (p0 - p) * ae;
+ }
+}
+
+void MEA::correctBaro(const Step &step)
{
- const auto filterState = filter.getState();
- const auto filterOutput = filter.getOutput();
-
- state.timestamp = TimestampTimer::getTimestamp();
- state.correctedPressure = filterOutput(0);
- state.x0 = filterState(0);
- state.x1 = filterState(1);
- state.x2 = filterState(2);
+ if (!step.hasCCPressure)
+ return;
+
+ float S = baroH * P * baroH.transpose() + baroR;
+ Matrix<float, 3, 1> K = (P * baroH.transpose()) / S;
+
+ P = (Matrix<float, 3, 3>::Identity() - K * baroH) * P;
+ x = x + K * (step.ccPressure - baroH * x);
}
-} // namespace Boardcore
+void MEA::correctAccel(const Step &step)
+{
+ if (!step.hasAcceleration || !step.hasSpeedAndAlt)
+ return;
+
+ // Do not correct at low speed/acceleration
+ if (step.acceleration.norm() < accelThresh ||
+ step.verticalSpeed < speedThresh)
+ return;
+
+ float y = (Kt * (float)(baroH * x) - force) / x(2);
+
+ Matrix<float, 1, 3> accelH =
+ Vector<float, 3>{Kt * baroH(0) / x(2), Kt * baroH(1) / x(2),
+ -(Kt * (float)(baroH * x) - force) / (x(2) * x(2))}
+ .transpose();
+
+ float accelR = alpha * q + c;
+
+ float S = accelH * P * accelH.transpose() + accelR;
+ Matrix<float, 3, 1> K = (P * accelH.transpose()) / S;
+
+ P = (Matrix<float, 3, 3>::Identity() - K * accelH) * P;
+ x = x + K * (step.acceleration.x() - y);
+}
+
+void MEA::computeApogee(const Step &step)
+{
+ if (!step.hasSpeedAndAlt)
+ return;
+
+ float mass = x(2);
+
+ // TODO: Matlab uses CD_correction_shutDown, should we?
+ // Holy fuck, massive formula, split this for the love of god
+ apogee = step.mslAltitude +
+ 1 / (2 * ((0.5f * rho * cd * crossSection) / mass)) *
+ log1p(1 + (step.verticalSpeed * step.verticalSpeed *
+ ((0.5f * rho * cd * crossSection) / mass)) /
+ Constants::g);
+}
+
+void MEA::updateState()
+{
+ state.timestamp = TimestampTimer::getTimestamp();
+
+ state.x0 = x(0);
+ state.x1 = x(1);
+
+ state.estimatedMass = x(2);
+ state.estimatedPressure = baroH * x;
+ state.estimatedApogee = apogee;
+}
\ No newline at end of file
diff --git a/src/shared/algorithms/MEA/MEA.h b/src/shared/algorithms/MEA/MEA.h
index d69bc5bb01b660d5cf8aea51e4a3cc5ebe936ee6..56467767a1ca4bd22a51456129a62f42cdf42baf 100644
--- a/src/shared/algorithms/MEA/MEA.h
+++ b/src/shared/algorithms/MEA/MEA.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -22,53 +22,124 @@
#pragma once
-#include <algorithms/Kalman/Kalman.h>
+#include <algorithms/MEA/MEAData.h>
+#include <sensors/SensorData.h>
+#include <utils/AeroUtils/AeroUtils.h>
-#include "MEAData.h"
+#include <Eigen/Dense>
namespace Boardcore
{
-/**
- * @brief MEA stands for Mass Estimation Algorithm.
- *
- * It represents a kalman filter which by performing a prediction with the
- * current feed valve state and a correction with the pressure in combustion
- * chamber, estimates the remaining mass of the rocket.
- */
class MEA
{
public:
- using KalmanFilter = Kalman<float, 3, 1, 1>;
+ struct Config
+ {
+ Eigen::Matrix<float, 3, 3> F; //< State propagation matrix
+ Eigen::Matrix<float, 3, 3> Q; //< Model variance matrix
+ Eigen::Vector<float, 3> G; //< Input vector
- explicit MEA(const KalmanFilter::KalmanConfig kalmanConfig);
+ Eigen::Vector<float, 3> baroH; //< Barometer output matrix
+ float baroR; //< Barometer measurement variance
- /**
- * @brief Update the Kalman filter.
- *
- * @param feedValvePosition Position reference of the feed valve [0-1]
- * @param pressure Measured pressure inside the combustion chamber [Pa]
- */
- void update(const float feedValvePosition, const float pressure);
+ Eigen::Matrix<float, 3, 3> P; //< Error covariance matrix
- /**
- * @brief Returns the MEA data including the estimated mass.
- */
- MEAState getState();
+ float initialMass; //< Initial mass of the rocket
+
+ float accelThresh; //< Minimum required acceleration to trigger accel
+ // correction.
+ float speedThresh; //< Minumum required speed to trigger accel
+ // correction.
+
+ float Kt; //< TODO: What is this?
+ float alpha; //< TODO: What is this?
+ float c; //< TODO: What is this?
+
+ Aeroutils::AerodynamicCoeff coeffs; //< Aerodynamic coefficients.
+ float crossSection; //< Cross section of the rocket.
+
+ float ae; //< TODO: What is this?
+ float p0; //< Pressure at nozzle exit
+ };
+
+ struct Step
+ {
+ float mainValveOpen =
+ 0.0f; //< How open is the main valve? (between 0-1)
+
+ float ccPressure =
+ 0.0f; //< What is the current combustion chamber pressure? [bar]
+ bool hasCCPressure = false; //< Is ccPressure valid?
+
+ Eigen::Vector<float, 3> acceleration =
+ Eigen::Vector<float, 3>::Zero(); //< Acceleration [m/s^2]
+ bool hasAcceleration = false; //< Is acceleration valid?
+
+ float verticalSpeed =
+ 0.0f; //< Vertical component of speed [up positive] [m/s]
+ float mslAltitude = 0.0f; //< Mean sea level altitude [m]
+ bool hasSpeedAndAlt =
+ false; //< Are verticalSpeed and mslAltitude valid?
+
+ explicit Step(float mainValveOpen);
+
+ void withCCPressure(PressureData ccPressure);
+ void withCCPressure(float ccPressure);
+
+ void withAcceleration(AccelerometerData acceleration);
+ void withAcceleration(Eigen::Vector<float, 3> acceleration);
- /**
- * @brief Sets the Kalman filter configuration.
- */
- void setKalmanConfig(KalmanFilter::KalmanConfig config);
+ void withSpeedAndAlt(float verticalSpeed, float mslAltitude);
+ };
+
+ explicit MEA(const Config &config);
+
+ void update(const Step &step);
+
+ MEAState getState();
private:
- /**
- * @brief Computes useful data from the kalman current state.
- */
+ void predict(const Step &step);
+ void computeForce(const Step &step);
+ void correctBaro(const Step &step);
+ void correctAccel(const Step &step);
+ void computeApogee(const Step &step);
void updateState();
- KalmanFilter filter;
+ Eigen::Matrix<float, 3, 3> F; //< State propagation matrix
+ Eigen::Matrix<float, 3, 3> Q; //< Model variance matrix
+
+ Eigen::Vector<float, 3> G; //< Input Vector
+
+ Eigen::Matrix<float, 1, 3> baroH;
+ float baroR;
+
+ Eigen::Matrix<float, 3, 3> P; //< Error covariance matrix
+ Eigen::Vector<float, 3> x; //< State vector
+
+ float mach = 0.0f; //< Latest computed mach
+ float cd = 0.0f; //< Latest computed CD
+ float rho = 0.0f; //< Latest computed rho
+ float q = 0.0f; //< Latest computed dynamic pressure
+ float force = 0.0f; //< Latest computed force
+
+ float apogee = 0.0f; //< Latest computed apogee
+
+ float accelThresh;
+ float speedThresh;
+
+ float Kt;
+ float alpha;
+ float c;
+
+ Aeroutils::AerodynamicCoeff coeffs;
+ float crossSection;
+
+ float ae;
+ float p0;
+
MEAState state;
};
-} // namespace Boardcore
+} // namespace Boardcore
\ No newline at end of file
diff --git a/src/shared/algorithms/MEA/MEAData.h b/src/shared/algorithms/MEA/MEAData.h
index 1a3dc708642c4c1dbfd246e494a7f7f297c5cabc..6164a8b7a9c2c1a85f8b5828e2128d653a66ce23 100644
--- a/src/shared/algorithms/MEA/MEAData.h
+++ b/src/shared/algorithms/MEA/MEAData.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Matteo Pignataro
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -22,6 +22,7 @@
#pragma once
+#include <cstdint>
#include <ostream>
namespace Boardcore
@@ -30,21 +31,25 @@ namespace Boardcore
struct MEAState
{
uint64_t timestamp;
- float correctedPressure;
+
+ float estimatedPressure;
+ float estimatedMass;
+ float estimatedApogee;
+
float x0;
float x1;
- float x2;
static std::string header()
{
- return "timestamp,correctedPressure,x0,x1,x2\n";
+ return "timestamp,estimatedPressure,estimatedMass,estimatedApogee,x0,"
+ "x1\n";
}
- void print(std::ostream& os) const
+ void print(std::ostream &os) const
{
- os << timestamp << "," << correctedPressure << "," << x0 << "," << x1
- << "," << x2 << "\n";
+ os << timestamp << "," << estimatedPressure << "," << estimatedMass
+ << "," << estimatedApogee << "," << x0 << "," << x1 << "\n";
}
};
-} // namespace Boardcore
+} // namespace Boardcore
\ No newline at end of file
diff --git a/src/shared/utils/AeroUtils/AeroUtils.cpp b/src/shared/utils/AeroUtils/AeroUtils.cpp
index 1ec63ecabc80c730de76342e82940fb388c69370..b1592df1e325c99ad024ff50f8a2ad0ae4aa7540 100644
--- a/src/shared/utils/AeroUtils/AeroUtils.cpp
+++ b/src/shared/utils/AeroUtils/AeroUtils.cpp
@@ -119,6 +119,13 @@ float computePitotAirspeed(float pressureTotal, float pressureStatic, float d,
return M * c;
}
+float computeCd(const AerodynamicCoeff& coeff, float mach)
+{
+ return coeff.n000 + coeff.n100 * mach + coeff.n200 * powf(mach, 2) +
+ coeff.n300 * powf(mach, 3) + coeff.n400 * powf(mach, 4) +
+ coeff.n500 * powf(mach, 5) + coeff.n600 * powf(mach, 6);
+}
+
} // namespace Aeroutils
} // namespace Boardcore
diff --git a/src/shared/utils/AeroUtils/AeroUtils.h b/src/shared/utils/AeroUtils/AeroUtils.h
index 96223d359399355a3a2f83621e647b9bfe0477c1..cc4a91d453744d790983144cf05cb2b0bf132e2b 100644
--- a/src/shared/utils/AeroUtils/AeroUtils.h
+++ b/src/shared/utils/AeroUtils/AeroUtils.h
@@ -231,6 +231,20 @@ float computePitotMach(float pressureTotal, float pressureStatic);
float computePitotAirspeed(float pressureTotal, float pressureStatic, float d,
float t0);
+struct AerodynamicCoeff
+{
+ float n000, n100, n200, n300, n400, n500, n600;
+};
+
+/**
+ * @brief Computes the CD from aerodynamic coefficients and mach.
+ *
+ * @param coeff Aerodynamic coefficients.
+ * @param mach Mach value.
+ * @return The computed CD.
+ */
+float computeCd(const AerodynamicCoeff& coeff, float mach);
+
} // namespace Aeroutils
} // namespace Boardcore
diff --git a/src/tests/catch/test-MEA.cpp b/src/tests/catch/test-MEA.cpp
index 6a4d64a68fb871a2d4173a7457f1e21756f1f445..257a52cf0cce2985d258745655b996987cf6c61e 100644
--- a/src/tests/catch/test-MEA.cpp
+++ b/src/tests/catch/test-MEA.cpp
@@ -30,6 +30,7 @@
using namespace Boardcore;
using namespace Eigen;
+/*
constexpr float sensorNoiseVariance = 0.36f;
constexpr float modelNoiseVariance = 0.1f;
constexpr float initialRocketMass = 35.01f;
@@ -40,17 +41,43 @@ MEA::KalmanFilter::KalmanConfig getMEAKalmanConfig()
// clang-format off
config.F = MEA::KalmanFilter::MatrixNN({
- {1.435871191228868, -0.469001276508780, 0.f},
+ {1.435871191228868, -0.469001276508780, 0.f},
{1.f, 0.f, 0.f},
{-0.002045309260755, 0.001867496708935, 1.f}});
-
+
config.H = {1.780138883879285,-1.625379384370081,0.f};
config.P = MEA::KalmanFilter::MatrixNN::Zero();
- config.Q = modelNoiseVariance * MEA::KalmanFilter::CVectorN({1, 1, 1}).asDiagonal();
- config.R[0] = sensorNoiseVariance;
- config.G = MEA::KalmanFilter::MatrixNM{{4}, {0}, {0}};
- config.x = {0, 0, initialRocketMass};
+ config.Q = modelNoiseVariance * MEA::KalmanFilter::CVectorN({1, 1,
+1}).asDiagonal(); config.R[0] = sensorNoiseVariance; config.G =
+MEA::KalmanFilter::MatrixNM{{4}, {0}, {0}}; config.x = {0, 0,
+initialRocketMass};
+ // clang-format on
+
+ return config;
+}
+*/
+
+MEA::Config getMEAConfig()
+{
+ MEA::Config config;
+
+ // clang-format off
+ config.F = Matrix<float, 3, 3>({
+ {1.435871191228868, -0.469001276508780, 0.f},
+ {1.f, 0.f, 0.f},
+ {-0.002045309260755, 0.001867496708935, 1.f}});
+ config.Q = Matrix<float, 3, 3>({
+ {0.1f, 0.0f, 0.0f},
+ {0.0f, 0.1f, 0.0f},
+ {0.0f, 0.0f, 0.1f}});
+ config.G = Matrix<float, 3, 1>{{4}, {0}, {0}};
+
+ config.baroH = {1.780138883879285,-1.625379384370081,0.f};
+ config.baroR = 0.36f;
+
+ config.P = Matrix<float, 3, 3>::Zero();
+ config.initialMass = 35.01f;
// clang-format on
return config;
@@ -58,31 +85,36 @@ MEA::KalmanFilter::KalmanConfig getMEAKalmanConfig()
TEST_CASE("MEA Update Test")
{
- MEA mea(getMEAKalmanConfig());
+ MEA mea(getMEAConfig());
MEAState state;
- std::cout << "pressure,estimatedPressure,estimatedMass,command"
- << std::endl;
+ std::cout << MEAState::header();
for (unsigned i = 1; i < PRESSURE.size(); i++)
{
// Update the kalman
- mea.update(COMMAND[i - 1], PRESSURE[i]);
+ MEA::Step step{COMMAND[i - 1]};
+ step.withCCPressure(PRESSURE[i]);
+
+ mea.update(step);
// Get the results
state = mea.getState();
- if (state.x2 != Approx(ESTIMATED_MASS[i]).epsilon(0.01))
+ state.print(std::cout);
+
+ if (state.estimatedMass != Approx(ESTIMATED_MASS[i]).epsilon(0.01))
{
FAIL("The estimated mass differs from the correct one ["
- << i << "]: " << state.x2 << " != " << ESTIMATED_MASS[i]);
+ << i << "]: " << state.estimatedMass
+ << " != " << ESTIMATED_MASS[i]);
}
- if (state.correctedPressure !=
+ if (state.estimatedPressure !=
Approx(ESTIMATED_PRESSURE[i]).epsilon(0.01))
{
FAIL("The estimated pressure differs from the correct one ["
- << i << "]: " << state.correctedPressure
+ << i << "]: " << state.estimatedPressure
<< " != " << ESTIMATED_PRESSURE[i]);
}
}