diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7610a2b91bfdfebbaca1893b38a71723d219680c..5278ccc51458b4bff955bfbefbbec214872f7089 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -131,6 +131,7 @@ add_executable(catch-tests-boardcore
src/tests/catch/test-sensormanager-catch.cpp
src/tests/catch/xbee/test-xbee-parser.cpp
src/tests/catch/test-modulemanager.cpp
+ src/tests/catch/test-MEA.cpp
)
target_compile_definitions(catch-tests-boardcore PRIVATE USE_MOCK_PERIPHERALS)
sbs_target(catch-tests-boardcore stm32f429zi_stm32f4discovery)
diff --git a/cmake/boardcore-host.cmake b/cmake/boardcore-host.cmake
index 355f4c9e69dca277517dd4a4bdc5cc401064433e..04fa5eef900162973d6f61c38850f4837aa3a80d 100644
--- a/cmake/boardcore-host.cmake
+++ b/cmake/boardcore-host.cmake
@@ -34,6 +34,9 @@ add_library(boardcore-host STATIC EXCLUDE_FROM_ALL
# Events
${SBS_BASE}/src/shared/events/EventBroker.cpp
+ # Algorithms
+ ${SBS_BASE}/src/shared/algorithms/MEA/MEA.cpp
+
# Logger
${SBS_BASE}/src/shared/logger/Logger.cpp
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index 2b621f0ac78ea8c67b4056aaf1b1a20575d8efcd..bea35c59bdc8ebffe8225932fddaa51e22ca9e9b 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -35,6 +35,7 @@ foreach(OPT_BOARD ${BOARDS})
# Algorithms
${SBS_BASE}/src/shared/algorithms/ADA/ADA.cpp
+ ${SBS_BASE}/src/shared/algorithms/MEA/MEA.cpp
${SBS_BASE}/src/shared/algorithms/AirBrakes/AirBrakes.cpp
${SBS_BASE}/src/shared/algorithms/AirBrakes/AirBrakesPI.cpp
${SBS_BASE}/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp
diff --git a/src/shared/algorithms/MEA/MEA.cpp b/src/shared/algorithms/MEA/MEA.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eeedf74c0eafe389a45b247a84d0e80da822c8d5
--- /dev/null
+++ b/src/shared/algorithms/MEA/MEA.cpp
@@ -0,0 +1,65 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "MEA.h"
+
+#include <drivers/timer/TimestampTimer.h>
+
+namespace Boardcore
+{
+
+MEA::MEA(const KalmanFilter::KalmanConfig kalmanConfig)
+ : filter(kalmanConfig), state()
+{
+ updateState();
+}
+
+void MEA::update(const float feedValvePosition, const float pressure)
+{
+ // Update the Kalman filter
+ filter.predictWithControl(KalmanFilter::CVectorM{feedValvePosition});
+ filter.correct(KalmanFilter::CVectorP{pressure});
+
+ // Compute the derived values for useful data
+ updateState();
+}
+
+MEAState MEA::getState() { return state; }
+
+void MEA::setKalmanConfig(KalmanFilter::KalmanConfig config)
+{
+ filter.setConfig(config);
+}
+
+void MEA::updateState()
+{
+ 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);
+}
+
+} // namespace Boardcore
diff --git a/src/shared/algorithms/MEA/MEA.h b/src/shared/algorithms/MEA/MEA.h
new file mode 100644
index 0000000000000000000000000000000000000000..d69bc5bb01b660d5cf8aea51e4a3cc5ebe936ee6
--- /dev/null
+++ b/src/shared/algorithms/MEA/MEA.h
@@ -0,0 +1,74 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <algorithms/Kalman/Kalman.h>
+
+#include "MEAData.h"
+
+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>;
+
+ explicit MEA(const KalmanFilter::KalmanConfig kalmanConfig);
+
+ /**
+ * @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);
+
+ /**
+ * @brief Returns the MEA data including the estimated mass.
+ */
+ MEAState getState();
+
+ /**
+ * @brief Sets the Kalman filter configuration.
+ */
+ void setKalmanConfig(KalmanFilter::KalmanConfig config);
+
+private:
+ /**
+ * @brief Computes useful data from the kalman current state.
+ */
+ void updateState();
+
+ KalmanFilter filter;
+ MEAState state;
+};
+
+} // namespace Boardcore
diff --git a/src/shared/algorithms/MEA/MEAData.h b/src/shared/algorithms/MEA/MEAData.h
new file mode 100644
index 0000000000000000000000000000000000000000..1a3dc708642c4c1dbfd246e494a7f7f297c5cabc
--- /dev/null
+++ b/src/shared/algorithms/MEA/MEAData.h
@@ -0,0 +1,50 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <ostream>
+
+namespace Boardcore
+{
+
+struct MEAState
+{
+ uint64_t timestamp;
+ float correctedPressure;
+ float x0;
+ float x1;
+ float x2;
+
+ static std::string header()
+ {
+ return "timestamp,correctedPressure,x0,x1,x2\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << correctedPressure << "," << x0 << "," << x1
+ << "," << x2 << "\n";
+ }
+};
+
+} // namespace Boardcore
diff --git a/src/tests/algorithms/MEA/test-mea-data.h b/src/tests/algorithms/MEA/test-mea-data.h
new file mode 100644
index 0000000000000000000000000000000000000000..9855b61ecd71c999566723c6ae679a851a4359b6
--- /dev/null
+++ b/src/tests/algorithms/MEA/test-mea-data.h
@@ -0,0 +1,281 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <vector>
+
+namespace Boardcore
+{
+
+// ------------------------ INPUT ------------------------
+static const std::vector<float> PRESSURE = {
+ -0.018727, -0.011994, -0.012176, -0.0097343, -0.012885, -0.011895,
+ -0.014315, -0.0097891, -0.010691, -0.013148, -0.011395, -0.010639,
+ -0.013105, -0.012073, -0.011279, -0.011302, -0.011906, -0.013049,
+ -0.011593, -0.012282, -0.012244, -0.013035, -0.010415, -0.012044,
+ -0.01411, -0.0092077, -0.0089086, -0.010949, -0.011459, -0.0090492,
+ -0.011111, -0.0070112, -0.0071889, -0.01109, -0.011112, -0.01189,
+ -0.0093305, -0.013255, -0.012205, -0.0095613, -0.012664, -0.0098792,
+ -0.0088024, -0.010518, -0.01152, -0.0094729, -0.010077, -0.0080807,
+ -0.0095873, -0.010477, -0.0078198, -0.0083045, -0.0098062, -0.0092916,
+ 0.037907, 0.24001, 1.0333, 3.1653, 6.5568, 10.218,
+ 13.217, 15.375, 15.996, 16.72, 17.983, 18.201,
+ 18.01, 16.48, 17.18, 16.497, 16.229, 16.601,
+ 15.282, 15.377, 15.213, 15.224, 14.735, 14.247,
+ 14.054, 14.831, 14.641, 14.393, 14.257, 14.956,
+ 15.592, 15.645, 14.798, 15.818, 15.853, 15.758,
+ 15.758, 15.647, 15.51, 16.56, 16.58, 15.891,
+ 15.887, 15.882, 17.436, 17.23, 16.343, 16.159,
+ 16.428, 18.798, 17.038, 18.658, 18.244, 17.652,
+ 17.149, 16.678, 17.084, 18.83, 18.766, 19.856,
+ 18.992, 17.264, 18.166, 18.412, 19.29, 20.958,
+ 21.309, 20.54, 21.449, 21.89, 21.224, 21.4,
+ 21.333, 21.325, 21.076, 21.816, 20.951, 22.139,
+ 20.418, 22.145, 20.161, 21.09, 20.991, 19.939,
+ 21.059, 20.28, 21.282, 19.841, 20.869, 21.239,
+ 20.521, 21.613, 20.785, 21.17, 20.411, 20.816,
+ 20.175, 20.505, 20.682, 20.565, 20.428, 20.848,
+ 20.166, 20.997, 19.955, 20.649, 20.61, 20.306,
+ 20.513, 19.99, 20.187, 20.209, 19.52, 20.172,
+ 19.656, 19.645, 19.618, 19.044, 19.281, 19.363,
+ 20.262, 18.984, 19.246, 19.587, 18.309, 18.158,
+ 19.416, 18.051, 19.359, 17.286, 17.632, 19.607,
+ 17.405, 18.506, 20.205, 17.558, 19.189, 18.391,
+ 17.968, 19.164, 17.625, 19.87, 18.357, 18.738,
+ 17.595, 19.368, 17.818, 17.404, 18.417, 18.575,
+ 17.781, 18.708, 18.537, 17.952, 18.19, 17.302,
+ 18.168, 17.657, 17.508, 16.681, 18.513, 17.824,
+ 18.308, 16.922, 18.563, 17.067, 16.971, 17.369,
+ 17.455, 19.174, 17.987, 17.34, 18.076, 16.953,
+ 18.469, 16.681, 17.889, 17.782, 16.265, 18.331,
+ 18.023, 17.036, 16.567, 17.961, 16.664, 16.531,
+ 17.637, 17.648, 17.222, 17.321, 16.506, 17.707,
+ 16.303, 18.207, 16.168, 16.939, 17.665, 16.497,
+ 18.031, 16.536, 17.645, 17.133, 17.118, 17.179,
+ 15.939, 17.202, 16.154, 17.158, 17.046, 16.353,
+ 17.694, 16.305, 16.197, 17.53, 15.979, 16.268,
+ 16.632, 17.1, 16.189, 16.505, 16.076, 15.862,
+ 16.456, 16.099, 16.633, 16.224, 17.538, 16.351,
+ 16.715, 17.034, 18.287, 18.959, 18.739, 18.808,
+ 18.751, 19.314, 19.394, 19.58, 19.555, 19.314,
+ 19.646, 18.998, 18.341, 18.391, 18.81, 18.718,
+ 18.903, 19.001, 19.149, 18.646, 18.773, 19.061,
+ 19.025, 18.477, 18.37, 18.343, 18.515, 17.954,
+ 17.349, 17.625, 18.109, 17.912, 17.947, 18.31,
+ 16.78, 16.194, 16.855, 17.456, 17.08, 16.839,
+ 18, 18.224, 18.317, 17.241, 16.661, 14.955,
+ 17.473, 16.875, 15.907, 15.414, 16.124, 15.985,
+ 15.51, 17.934, 17.329, 16.454, 16.689, 17.068,
+ 15.956, 15.862, 15.669, 15.948, 17.338, 16.543,
+ 15.87, 16.74, 15.95, 16.627, 15.848, 15.605,
+ 16.175, 15.999, 17.087, 16.031, 16.561, 16.818,
+ 16.243, 15.221, 16.076, 15.677, 14.82, 15.273,
+ 16.359, 15.907, 14.592, 15.33, 15.927, 16.8,
+ 15.997, 16.704, 14.614, 15.325, 14.792, 15.29,
+ 15.268, 15.095, 15.119, 16.207, 15.773, 14.714,
+ 14.576, 15.867, 15.65, 16.356, 14.951, 14.956,
+ 15.191, 14.95, 15.646, 15.082, 14.509, 14.743,
+ 14.795, 15.542, 15.602, 15.148, 14.182, 14.007,
+ 13.674, 14.01, 13.538, 13.575, 12.123, 11.277,
+ 9.9151, 8.247, 6.7411, 5.4387, 4.3453, 3.4388,
+ 2.6996, 2.1099, 1.6294, 1.2363, 0.91788, 0.65743,
+ 0.45639, 0.30189, 0.18781, 0.1077, 0.05356, 0.024951,
+ 0.013025, 0.012876, 0.0078332, 0.0087931, 0.0064979, 0.005332,
+ 0.0054313, 0.0060629, 0.0089005, 0.005985, 0.0063222, 0.0039524,
+ 0.0027762, 0.0039834, 0.0053081, 0.010477, 0.011662, 0.011666,
+ 0.0094336, 0.0087342, 0.0068955, -0.00084894, -0.0015196, 0.00066334,
+ 0.0043813, 0.004087, 0.00071604, 0.00095268, 0.004491, 0.0032795,
+ 0.0032874, 0.003815, 0.0047803, 0.0040967, 0.0045046, 0.0069973,
+ 0.0041161, 0.0029485, 0.0032241, 0.0020196, 0.004437, 0.0056916,
+ 0.0041087, 0.0055695, 0.0041131};
+
+static const std::vector<float> COMMAND = {
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.20958, 0.3997, 0.58982, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 1, 1, 1, 1, 1, 1, 1, 1, 0.57085, 0.38081, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0};
+
+// ------------------------ EXPECTED OUTPUT ------------------------
+static const std::vector<float> ESTIMATED_PRESSURE = {
+ 0, -0.0074056, -0.0091778, -0.0079698, -0.009788, -0.0095103,
+ -0.011032, -0.0084319, -0.0085461, -0.010156, -0.0093438, -0.0087227,
+ -0.0102, -0.0098228, -0.0092555, -0.0091744, -0.0095543, -0.010365,
+ -0.0095904, -0.0098966, -0.0099356, -0.010457, -0.0088793, -0.009637,
+ -0.011105, -0.0082391, -0.0075198, -0.0086931, -0.0092367, -0.0077936,
+ -0.0088485, -0.0064143, -0.0060749, -0.0085079, -0.0089701, -0.0095564,
+ -0.0080267, -0.010262, -0.010005, -0.0082665, -0.0099357, -0.0084606,
+ 0.52771, 1.1135, 1.7114, 2.878, 3.105, 3.1606,
+ 3.1816, 3.1957, 3.2099, 3.2213, 3.2309, 3.2406,
+ 3.2798, 3.4232, 3.9648, 5.4392, 7.8964, 10.713,
+ 13.178, 15.045, 15.818, 16.456, 17.415, 17.761,
+ 17.732, 16.772, 17.067, 16.703, 16.482, 16.696,
+ 15.905, 15.832, 15.724, 15.72, 15.414, 15.052,
+ 14.867, 15.335, 15.305, 15.145, 15.032, 15.462,
+ 15.954, 16.083, 15.568, 16.131, 16.262, 16.229,
+ 16.228, 16.16, 16.063, 16.721, 16.859, 16.447,
+ 16.372, 16.357, 17.353, 17.409, 16.855, 16.637,
+ 16.772, 18.319, 17.481, 18.369, 18.272, 17.879,
+ 17.488, 17.116, 17.309, 18.465, 18.641, 19.377,
+ 18.964, 17.785, 18.148, 18.374, 18.981, 20.166,
+ 20.615, 20.213, 20.728, 21.112, 20.762, 20.817,
+ 20.79, 20.784, 20.628, 21.078, 20.61, 21.289,
+ 20.314, 21.244, 20.147, 20.542, 20.552, 19.881,
+ 20.475, 20.086, 20.657, 19.839, 20.348, 20.679,
+ 20.281, 20.908, 20.494, 20.666, 20.212, 20.388,
+ 20.009, 20.151, 20.29, 20.24, 20.143, 20.395,
+ 20.003, 20.464, 19.881, 20.218, 20.254, 20.066,
+ 20.164, 19.846, 19.913, 19.939, 19.501, 19.837,
+ 19.567, 19.509, 19.48, 19.104, 19.185, 19.251,
+ 19.837, 19.126, 19.161, 19.385, 18.605, 18.363,
+ 19.121, 18.384, 19.084, 17.883, 17.88, 19.142,
+ 17.962, 18.448, 19.625, 18.146, 18.918, 18.548,
+ 18.207, 18.91, 18.052, 19.332, 18.598, 18.707,
+ 17.994, 18.998, 18.189, 17.773, 18.343, 18.549,
+ 18.077, 18.584, 18.567, 18.189, 18.271, 17.716,
+ 18.167, 17.921, 17.779, 17.221, 18.29, 18.044,
+ 18.309, 17.469, 18.366, 17.571, 17.362, 17.576,
+ 17.67, 18.788, 18.235, 17.718, 18.095, 17.444,
+ 18.295, 17.305, 17.896, 17.936, 16.97, 18.115,
+ 18.128, 17.498, 17.081, 17.896, 17.214, 17.002,
+ 17.67, 17.799, 17.55, 17.568, 17.048, 17.721,
+ 16.944, 18.02, 16.912, 17.2, 17.718, 17.065,
+ 17.927, 17.128, 17.691, 17.467, 17.416, 17.445,
+ 16.655, 17.318, 16.767, 17.308, 17.335, 16.896,
+ 17.674, 16.927, 16.719, 17.534, 16.69, 16.718,
+ 16.956, 17.299, 16.778, 16.883, 16.627, 16.441,
+ 16.786, 16.62, 16.931, 16.725, 17.53, 16.918,
+ 17.039, 17.266, 18.112, 18.701, 18.673, 18.717,
+ 18.692, 19.052, 19.174, 19.32, 19.335, 19.187,
+ 19.376, 18.998, 18.51, 18.453, 18.711, 18.701,
+ 18.818, 18.904, 19.016, 18.715, 18.743, 18.933,
+ 18.945, 18.598, 18.465, 18.422, 18.525, 18.183,
+ 17.732, 17.823, 18.148, 18.081, 18.09, 18.324,
+ 17.385, 16.834, 17.153, 17.594, 17.433, 17.247,
+ 17.954, 18.229, 18.339, 17.671, 17.175, 15.988,
+ 17.379, 17.25, 16.604, 16.166, 16.537, 16.514,
+ 16.202, 17.696, 17.584, 17.003, 17.046, 17.297,
+ 16.63, 16.445, 16.286, 16.433, 17.349, 17.009,
+ 16.514, 16.98, 16.559, 16.915, 16.48, 16.243,
+ 16.563, 16.508, 17.195, 16.645, 16.884, 17.092,
+ 16.763, 16.047, 16.461, 16.281, 15.696, 15.876,
+ 16.604, 16.448, 15.576, 15.885, 16.324, 16.964,
+ 16.568, 16.949, 15.68, 15.9, 15.598, 15.859,
+ 15.892, 15.786, 15.78, 16.476, 16.326, 15.62,
+ 15.4, 16.185, 16.19, 16.644, 15.828, 15.681,
+ 15.803, 15.67, 16.091, 14.711, 13.597, 12.561,
+ 12.388, 12.822, 12.929, 12.647, 11.965, 11.717,
+ 11.446, 11.601, 11.317, 11.279, 10.332, 9.6043,
+ 8.5851, 7.314, 6.0981, 5.0207, 4.1012, 3.3296,
+ 2.6921, 2.1751, 1.7506, 1.3997, 1.111, 0.87169,
+ 0.68052, 0.52903, 0.41178, 0.32364, 0.25853, 0.21504,
+ 0.18721, 0.17076, 0.15417, 0.14222, 0.12981, 0.11875,
+ 0.10939, 0.10128, 0.095369, 0.086681, 0.080029, 0.072441,
+ 0.065835, 0.061296, 0.057545, 0.056711, 0.05415, 0.050774,
+ 0.04605, 0.042273, 0.038137, 0.030327, 0.026532, 0.025457,
+ 0.026013, 0.024435, 0.020614, 0.018799, 0.019572, 0.017874,
+ 0.01659, 0.015793, 0.015441, 0.014182, 0.013516, 0.014353,
+ 0.012077, 0.01037, 0.0097336, 0.0083853, 0.009264, 0.0098445,
+ 0.0085847, 0.008965, 0.0078043};
+
+static const std::vector<float> ESTIMATED_MASS = {
+ 35.01, 35.01, 35.01, 35.01, 35.01, 35.01, 35.01, 35.01, 35.01,
+ 35.01, 35.01, 35.01, 35.01, 35.01, 35.01, 35.01, 35.01, 35.01,
+ 35.01, 35.01, 35.01, 35.01, 35.01, 35.01, 35.01, 35.01, 35.01,
+ 35.01, 35.01, 35.01, 35.01, 35.01, 35.01, 35.01, 35.01, 35.01,
+ 35.01, 35.01, 35.01, 35.01, 35.01, 35.01, 35.01, 35.01, 35.01,
+ 35.009, 35.008, 35.006, 35.004, 35.002, 35.001, 34.999, 34.997, 34.995,
+ 34.993, 34.991, 34.989, 34.986, 34.98, 34.971, 34.959, 34.944, 34.926,
+ 34.908, 34.889, 34.869, 34.848, 34.828, 34.809, 34.789, 34.77, 34.751,
+ 34.732, 34.714, 34.696, 34.679, 34.661, 34.644, 34.627, 34.61, 34.593,
+ 34.576, 34.559, 34.542, 34.524, 34.506, 34.488, 34.47, 34.452, 34.434,
+ 34.415, 34.397, 34.379, 34.36, 34.341, 34.322, 34.304, 34.285, 34.266,
+ 34.246, 34.227, 34.208, 34.189, 34.169, 34.148, 34.128, 34.107, 34.086,
+ 34.066, 34.046, 34.026, 34.006, 33.985, 33.963, 33.941, 33.919, 33.899,
+ 33.878, 33.857, 33.835, 33.811, 33.787, 33.764, 33.739, 33.715, 33.691,
+ 33.666, 33.642, 33.618, 33.594, 33.569, 33.545, 33.521, 33.497, 33.473,
+ 33.449, 33.425, 33.402, 33.378, 33.355, 33.331, 33.308, 33.284, 33.261,
+ 33.237, 33.213, 33.189, 33.165, 33.141, 33.118, 33.094, 33.071, 33.048,
+ 33.024, 33.001, 32.977, 32.954, 32.931, 32.907, 32.884, 32.861, 32.837,
+ 32.814, 32.791, 32.768, 32.745, 32.722, 32.699, 32.676, 32.654, 32.631,
+ 32.609, 32.587, 32.565, 32.542, 32.52, 32.498, 32.475, 32.453, 32.432,
+ 32.411, 32.389, 32.368, 32.346, 32.326, 32.305, 32.283, 32.263, 32.241,
+ 32.219, 32.198, 32.176, 32.155, 32.134, 32.113, 32.092, 32.069, 32.048,
+ 32.027, 32.006, 31.984, 31.964, 31.943, 31.922, 31.901, 31.88, 31.859,
+ 31.838, 31.817, 31.796, 31.776, 31.755, 31.734, 31.714, 31.694, 31.673,
+ 31.653, 31.632, 31.612, 31.591, 31.571, 31.551, 31.531, 31.511, 31.489,
+ 31.468, 31.448, 31.428, 31.407, 31.387, 31.367, 31.346, 31.326, 31.307,
+ 31.286, 31.265, 31.246, 31.226, 31.206, 31.186, 31.167, 31.146, 31.126,
+ 31.106, 31.086, 31.067, 31.047, 31.027, 31.007, 30.987, 30.968, 30.948,
+ 30.928, 30.908, 30.888, 30.868, 30.848, 30.828, 30.809, 30.79, 30.77,
+ 30.751, 30.731, 30.712, 30.692, 30.672, 30.653, 30.634, 30.614, 30.595,
+ 30.576, 30.557, 30.537, 30.518, 30.499, 30.48, 30.462, 30.443, 30.424,
+ 30.404, 30.385, 30.365, 30.346, 30.327, 30.307, 30.286, 30.264, 30.243,
+ 30.221, 30.2, 30.178, 30.155, 30.133, 30.111, 30.089, 30.066, 30.045,
+ 30.023, 30.002, 29.981, 29.959, 29.937, 29.916, 29.894, 29.872, 29.851,
+ 29.829, 29.807, 29.786, 29.765, 29.744, 29.722, 29.702, 29.682, 29.661,
+ 29.64, 29.62, 29.599, 29.578, 29.559, 29.539, 29.52, 29.5, 29.48,
+ 29.46, 29.439, 29.419, 29.398, 29.378, 29.359, 29.34, 29.32, 29.301,
+ 29.282, 29.264, 29.245, 29.227, 29.208, 29.188, 29.168, 29.149, 29.129,
+ 29.11, 29.091, 29.072, 29.054, 29.035, 29.015, 28.996, 28.977, 28.958,
+ 28.939, 28.92, 28.902, 28.883, 28.864, 28.846, 28.826, 28.807, 28.788,
+ 28.769, 28.75, 28.732, 28.713, 28.695, 28.677, 28.659, 28.64, 28.622,
+ 28.604, 28.586, 28.568, 28.549, 28.53, 28.511, 28.493, 28.475, 28.458,
+ 28.44, 28.422, 28.404, 28.386, 28.368, 28.349, 28.332, 28.314, 28.296,
+ 28.278, 28.259, 28.241, 28.224, 28.206, 28.188, 28.169, 28.152, 28.135,
+ 28.119, 28.103, 28.087, 28.071, 28.055, 28.04, 28.025, 28.011, 27.996,
+ 27.982, 27.968, 27.955, 27.943, 27.933, 27.924, 27.917, 27.911, 27.906,
+ 27.902, 27.899, 27.897, 27.895, 27.893, 27.892, 27.891, 27.891, 27.89,
+ 27.89, 27.89, 27.889, 27.889, 27.889, 27.889, 27.889, 27.889, 27.889,
+ 27.889, 27.889, 27.889, 27.889, 27.888, 27.888, 27.888, 27.888, 27.888,
+ 27.888, 27.888, 27.888, 27.888, 27.888, 27.888, 27.888, 27.888, 27.888,
+ 27.888, 27.888, 27.888, 27.888, 27.888, 27.888, 27.888, 27.888, 27.888,
+ 27.888, 27.888, 27.888, 27.888, 27.888, 27.888, 27.888, 27.888, 27.888,
+ 27.888, 27.888, 27.888};
+
+} // namespace Boardcore
diff --git a/src/tests/catch/test-MEA.cpp b/src/tests/catch/test-MEA.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6a4d64a68fb871a2d4173a7457f1e21756f1f445
--- /dev/null
+++ b/src/tests/catch/test-MEA.cpp
@@ -0,0 +1,89 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <algorithms/MEA/MEA.h>
+
+#include <catch2/catch.hpp>
+#include <iostream>
+
+#include "../algorithms/MEA/test-mea-data.h"
+
+using namespace Boardcore;
+using namespace Eigen;
+
+constexpr float sensorNoiseVariance = 0.36f;
+constexpr float modelNoiseVariance = 0.1f;
+constexpr float initialRocketMass = 35.01f;
+
+MEA::KalmanFilter::KalmanConfig getMEAKalmanConfig()
+{
+ MEA::KalmanFilter::KalmanConfig config;
+
+ // clang-format off
+ config.F = MEA::KalmanFilter::MatrixNN({
+ {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};
+ // clang-format on
+
+ return config;
+}
+
+TEST_CASE("MEA Update Test")
+{
+ MEA mea(getMEAKalmanConfig());
+ MEAState state;
+
+ std::cout << "pressure,estimatedPressure,estimatedMass,command"
+ << std::endl;
+
+ for (unsigned i = 1; i < PRESSURE.size(); i++)
+ {
+ // Update the kalman
+ mea.update(COMMAND[i - 1], PRESSURE[i]);
+
+ // Get the results
+ state = mea.getState();
+
+ if (state.x2 != Approx(ESTIMATED_MASS[i]).epsilon(0.01))
+ {
+ FAIL("The estimated mass differs from the correct one ["
+ << i << "]: " << state.x2 << " != " << ESTIMATED_MASS[i]);
+ }
+
+ if (state.correctedPressure !=
+ Approx(ESTIMATED_PRESSURE[i]).epsilon(0.01))
+ {
+ FAIL("The estimated pressure differs from the correct one ["
+ << i << "]: " << state.correctedPressure
+ << " != " << ESTIMATED_PRESSURE[i]);
+ }
+ }
+}
\ No newline at end of file