[MEA] Fixed bugs and added some documentation

- Initialized mass parameter
- Fixed apogee estimation massive formula
- Simplified apogee estimation formula splitting it
- Added some documentation

src/shared/algorithms/MEA/MEA.cpp

@@ -61,10 +61,11 @@ void MEA::Step::withSpeedAndAlt(float verticalSpeed, float mslAltitude)
 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},
-      minMass{config.minMass}, maxMass{config.maxMass}
+      mass{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}, minMass{config.minMass}, maxMass{
+                                                                 config.maxMass}
 {
     updateState();
 }
@@ -181,12 +182,12 @@ void MEA::computeApogee(const Step &step)
     if (!step.hasSpeedAndAlt)
         return;
 
-    // TODO: Matlab uses CD_correction_shutDown, should we?
-    // Holy fuck, massive formula, split this for the love of god
+    // Simplified massive formula for apogee estimation.
+    // Warning: log1p(x) = log(1 + x)
+    float temp = ((rho * cd * crossSection) / mass);
     apogee     = step.mslAltitude +
-             1 / (2 * ((0.5f * rho * cd * crossSection) / mass)) *
-                 log1p(1 + (step.verticalSpeed * step.verticalSpeed *
-                            ((0.5f * rho * cd * crossSection) / mass)) /
+             1 / temp *
+                 log1p(0.5 * (step.verticalSpeed * step.verticalSpeed * temp) /
                        Constants::g);
 }
 

src/shared/algorithms/MEA/MEA.h

@@ -128,8 +128,8 @@ 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 mass;           //< Latest computed mass
 
     float accelThresh;
     float speedThresh;

src/shared/algorithms/MEA/MEAData.h

@@ -32,20 +32,19 @@ struct MEAState
 {
     uint64_t timestamp;
 
-    float estimatedPressure;
-    float estimatedMass;
-    float estimatedApogee;
-    float estimatedForce;
+    float estimatedPressure;  ///< Estimated pressure in combustion chamber [Pa]
+    float estimatedMass;      ///< Estimated rocket mass [kg]
+    float estimatedApogee;    ///< Estimated apogee in msl [m]
+    float estimatedForce;     ///< Estimated drag force [N]
 
-    float x0;
-    float x1;
-    float x2;
+    float x0;  ///< first kalman state
+    float x1;  ///< second kalman state
+    float x2;  ///< third kalman state representing the mass
 
     static std::string header()
     {
         return "timestamp,estimatedPressure,estimatedMass,estimatedApogee,"
-               "estimatedForce,x0,"
-               "x1,x2\n";
+               "estimatedForce,x0,x1,x2\n";
     }
 
     void print(std::ostream &os) const