[MEA] Added mass clamping

src/shared/algorithms/MEA/MEA.cpp

@@ -63,7 +63,8 @@ MEA::MEA(const Config &config)
       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}
+      crossSection{config.crossSection}, ae{config.ae}, p0{config.p0},
+      minMass{config.minMass}, maxMass{config.maxMass}
 {
 }
 
@@ -81,6 +82,9 @@ void MEA::update(const Step &step)
     // Run accelerometer correction
     correctAccel(step);
 
+    // Compute current mass
+    computeMass();
+
     // Run apogee prediction
     computeApogee(step);
 
@@ -164,13 +168,18 @@ void MEA::correctAccel(const Step &step)
     x = x + K * (step.acceleration.x() - y);
 }
 
+void MEA::computeMass()
+{
+    // Clamp the used mass, so that we don't use bogus values in case the filter
+    // fails BAD
+    mass = std::max(std::min(x(2), maxMass), minMass);
+}
+
 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 +
@@ -186,8 +195,9 @@ void MEA::updateState()
 
     state.x0 = x(0);
     state.x1 = x(1);
+    state.x2 = x(2);
 
-    state.estimatedMass     = x(2);
+    state.estimatedMass     = mass;
     state.estimatedPressure = baroH * x;
     state.estimatedApogee   = apogee;
     state.estimatedForce    = force;

src/shared/algorithms/MEA/MEA.h

@@ -61,6 +61,9 @@ public:
 
         float ae;  //< Efflux area
         float p0;  //< Pressure at nozzle exit
+
+        float minMass;  //< Minimum mass used for predicted apogee
+        float maxMass;  //< Maximum mass used for predicted apogee
     };
 
     struct Step
@@ -104,6 +107,7 @@ private:
     void computeForce(const Step &step);
     void correctBaro(const Step &step);
     void correctAccel(const Step &step);
+    void computeMass();
     void computeApogee(const Step &step);
     void updateState();
 
@@ -124,6 +128,7 @@ private:
     float q     = 0.0f;  //< Latest computed dynamic pressure
     float force = 0.0f;  //< Latest computed force
 
+    float mass   = 0.0f;  //< Latest computed mass
     float apogee = 0.0f;  //< Latest computed apogee
 
     float accelThresh;
@@ -139,6 +144,9 @@ private:
     float ae;
     float p0;
 
+    float minMass;
+    float maxMass;
+
     MEAState state;
 };
 

src/shared/algorithms/MEA/MEAData.h

@@ -39,19 +39,20 @@ struct MEAState
 
     float x0;
     float x1;
+    float x2;
 
     static std::string header()
     {
         return "timestamp,estimatedPressure,estimatedMass,estimatedApogee,"
                "estimatedForce,x0,"
-               "x1\n";
+               "x1,x2\n";
     }
 
     void print(std::ostream &os) const
     {
         os << timestamp << "," << estimatedPressure << "," << estimatedMass
            << "," << estimatedApogee << "," << estimatedForce << "," << x0
-           << "," << x1 << "\n";
+           << "," << x1 << "," << x2 << "\n";
     }
 };
 

src/tests/catch/test-MEA.cpp

@@ -78,6 +78,9 @@ MEA::Config getMEAConfig()
 
     config.P           = Matrix<float, 3, 3>::Zero();
     config.initialMass = 35.01f;
+
+    config.minMass = 20.0f;
+    config.maxMass = 40.0f;
     // clang-format on
 
     return config;