Switched GLE from running average to exponential moving average

include/state_estimation/GroundLevelEstimator.h

@@ -35,7 +35,7 @@ class GroundLevelEstimator{
          * Stops recording ground level measurements and freezes the EGL.
          * Should be called once when launch is confirmed.
          */
-        void launchDeteched();
+        void launchDetected();
 
         /**
          * @brief Gets the estimated ground level.
@@ -49,6 +49,7 @@ class GroundLevelEstimator{
         bool launched = false; //Turned true if launch is detected
         float estimatedGroundLevel_m = 0.0F; //EGL in meters
         uint32_t sampleCount = 0; //Number of samples used for ground level estimate
+        float alpha; //Determines how much weight the most recent number added has on the current EGL
 
 };
 

src/state_estimation/GroundLevelEstimator.cpp

@@ -2,16 +2,20 @@
 
 // Constructor
 GroundLevelEstimator::GroundLevelEstimator()
-: launched(false), estimatedGroundLevel_m(0.0F), sampleCount(0)
+: launched(false), estimatedGroundLevel_m(0.0F), sampleCount(0), alpha(0.1F)
 {}
 
 // Update the ground level estimate or convert ASL to AGL - Altitude ABOVE ground level
 float GroundLevelEstimator::update(float currentASL_m) {
-
     // Before launch: accumulate samples to estimate ground level
     if (!launched) {
-        // Running average of ground level samples
-        estimatedGroundLevel_m = ((estimatedGroundLevel_m * sampleCount) + currentASL_m) / (sampleCount + 1);
+        if (sampleCount == 0) {
+            // Initialize with first sample
+            estimatedGroundLevel_m = currentASL_m;
+        } else {
+            // Exponential moving average: EMA = alpha * newValue + (1 - alpha) * oldEMA
+            estimatedGroundLevel_m = (alpha * currentASL_m) + ((1.0F - alpha) * estimatedGroundLevel_m);
+        }
         sampleCount++;
 
         // Still on ground, so AGL is 0
@@ -23,7 +27,7 @@ float GroundLevelEstimator::update(float currentASL_m) {
 }
 
 // Signal that launch has been detected
-void GroundLevelEstimator::launchDeteched() {
+void GroundLevelEstimator::launchDetected() {
     launched = true;
     // Ground level estimate is now frozen at estimatedGroundLevel_m
 }