diff --git a/include/state_estimation/GroundLevelEstimator.h b/include/state_estimation/GroundLevelEstimator.h index 2bae780..4c32af5 100644 --- a/include/state_estimation/GroundLevelEstimator.h +++ b/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 }; diff --git a/src/state_estimation/GroundLevelEstimator.cpp b/src/state_estimation/GroundLevelEstimator.cpp index 31d5e0c..88c4d6b 100644 --- a/src/state_estimation/GroundLevelEstimator.cpp +++ b/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 }