From 4343ea26f854efbcce4ecfb62c7e9c89bfa37541 Mon Sep 17 00:00:00 2001 From: Logan Rundle <154925928+LoganRundle-1@users.noreply.github.com> Date: Thu, 19 Feb 2026 19:22:02 -0500 Subject: [PATCH 1/5] updated apogee predicted to be more accurate by adding analytic_update function --- include/state_estimation/ApogeePredictor.h | 6 +++ src/state_estimation/ApogeePredictor.cpp | 51 ++++++++++++++++++++++ 2 files changed, 57 insertions(+) diff --git a/include/state_estimation/ApogeePredictor.h b/include/state_estimation/ApogeePredictor.h index 3067a3b..1d4e112 100644 --- a/include/state_estimation/ApogeePredictor.h +++ b/include/state_estimation/ApogeePredictor.h @@ -5,6 +5,7 @@ #include "state_estimation/VerticalVelocityEstimator.h" + /** * @brief Predicts time‑to‑apogee and altitude‑at‑apogee from real‑time kinematics. * @@ -42,12 +43,16 @@ class ApogeePredictor { void poly_update(); + void analytic_update(); + // ----- Accessors ----- [[nodiscard]] bool isPredictionValid() const; [[nodiscard]] float getTimeToApogee_s() const; [[nodiscard]] uint32_t getPredictedApogeeTimestamp_ms() const; [[nodiscard]] float getPredictedApogeeAltitude_m() const; [[nodiscard]] float getFilteredDeceleration() const; + [[nodiscard]] float getdragCoefficient() const; + private: const VerticalVelocityEstimator& vve_; @@ -61,6 +66,7 @@ class ApogeePredictor { float tToApogee_; ///< Time until apogee (seconds) uint32_t predApogeeTs_; ///< Timestamp of predicted apogee (ms) float predApogeeAlt_;///< Predicted altitude at apogee (m) + float k_ = 0.0025F; ///< Drag coefficient // Bookkeeping uint32_t lastTs_; ///< Last timestamp received diff --git a/src/state_estimation/ApogeePredictor.cpp b/src/state_estimation/ApogeePredictor.cpp index 0ef0cb2..96d7ea5 100644 --- a/src/state_estimation/ApogeePredictor.cpp +++ b/src/state_estimation/ApogeePredictor.cpp @@ -21,6 +21,9 @@ constexpr float MAGIC_HALF = 0.5F; constexpr float GRAVITY_MS2 = 9.80665F; constexpr uint32_t MAX_WARMUPS = 1000; +float dragRatioFiltered_ = 0.0F; // k +bool dragLocked_ = false; + } // namespace ApogeePredictor::ApogeePredictor(const VerticalVelocityEstimator& velocityEstimator, @@ -169,6 +172,50 @@ void ApogeePredictor::poly_update() { // currentTimestamp_ms, altitude_m, velocity_ms, acceleration_ms2, apogeeRemaining_m, delta_h_simple); } +void ApogeePredictor::analytic_update() +{ + //gets the gravity constant and the current velocity and altitude of the rocket + const float g = 9.80665F; + float v = vve_.getEstimatedVelocity(); + float h = vve_.getEstimatedAltitude(); + + //if the velocity is less than or equal to zero, the rocket has already reach apogee and the apogee is the current altitude + if (v <= 0.0F) + { + predApogeeAlt_ = h; + valid_ = true; + return; + } + + //gets the current acceleration of the rocket + float a = vve_.getInertialVerticalAcceleration(); + + //calculates the measured drag coefficient + float kMeasured = -(a + g) / (v*v + 0.001F); + + if (kMeasured > 0.0F && kMeasured < 1.0F) + { + float alpha = clamp(fabs(v) / 150.0F, 0.02F, 0.25F); + k_ = (1.0F - alpha) * k_ + alpha * kMeasured; + } + + // Analytic apogee calculation (instant, no simulation) + float apogee = 0.0F; + + if (k_ > 0.00001F) + { + apogee = h + (0.5F / k_) * + logf((g + k_ * v * v) / g); + } + else + { + // fallback if drag unknown + apogee = h + (v * v) / (2.0F * g); + } + + predApogeeAlt_ = apogee; + valid_ = true; +} // Simple getters @@ -189,3 +236,7 @@ float ApogeePredictor::getPredictedApogeeAltitude_m() const { float ApogeePredictor::getFilteredDeceleration() const { return filteredDecel_; } + +float ApogeePredictor::getdragCoefficient() const { + return k_; +} \ No newline at end of file From 48460111d494c7f53a40016902c2e0c6ad480693 Mon Sep 17 00:00:00 2001 From: Logan Rundle <154925928+LoganRundle-1@users.noreply.github.com> Date: Sat, 21 Feb 2026 11:02:57 -0500 Subject: [PATCH 2/5] added comments to new functions --- src/state_estimation/ApogeePredictor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/state_estimation/ApogeePredictor.cpp b/src/state_estimation/ApogeePredictor.cpp index 96d7ea5..2e8f105 100644 --- a/src/state_estimation/ApogeePredictor.cpp +++ b/src/state_estimation/ApogeePredictor.cpp @@ -199,7 +199,7 @@ void ApogeePredictor::analytic_update() k_ = (1.0F - alpha) * k_ + alpha * kMeasured; } - // Analytic apogee calculation (instant, no simulation) + // Analytic apogee calculation float apogee = 0.0F; if (k_ > 0.00001F) From 52c78f8b3e1cb45b6ba83c603a8df1f6e659aba6 Mon Sep 17 00:00:00 2001 From: Logan Rundle <154925928+LoganRundle-1@users.noreply.github.com> Date: Sat, 21 Feb 2026 11:07:25 -0500 Subject: [PATCH 3/5] fix an error with data type --- src/state_estimation/ApogeePredictor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/state_estimation/ApogeePredictor.cpp b/src/state_estimation/ApogeePredictor.cpp index 2e8f105..58d6220 100644 --- a/src/state_estimation/ApogeePredictor.cpp +++ b/src/state_estimation/ApogeePredictor.cpp @@ -195,7 +195,7 @@ void ApogeePredictor::analytic_update() if (kMeasured > 0.0F && kMeasured < 1.0F) { - float alpha = clamp(fabs(v) / 150.0F, 0.02F, 0.25F); + float alpha = clamp(std::fabs(v) / 150.0F, 0.02F, 0.25F); k_ = (1.0F - alpha) * k_ + alpha * kMeasured; } From 88c0476678bd620438f2a3a78c76db1683256772 Mon Sep 17 00:00:00 2001 From: Logan Rundle <154925928+LoganRundle-1@users.noreply.github.com> Date: Sat, 21 Feb 2026 11:34:58 -0500 Subject: [PATCH 4/5] fixed variable names and added new test with pass fail --- src/state_estimation/ApogeePredictor.cpp | 41 ++++++++++++++++-------- 1 file changed, 28 insertions(+), 13 deletions(-) diff --git a/src/state_estimation/ApogeePredictor.cpp b/src/state_estimation/ApogeePredictor.cpp index 58d6220..02f1394 100644 --- a/src/state_estimation/ApogeePredictor.cpp +++ b/src/state_estimation/ApogeePredictor.cpp @@ -172,52 +172,67 @@ void ApogeePredictor::poly_update() { // currentTimestamp_ms, altitude_m, velocity_ms, acceleration_ms2, apogeeRemaining_m, delta_h_simple); } + void ApogeePredictor::analytic_update() { //gets the gravity constant and the current velocity and altitude of the rocket - const float g = 9.80665F; - float v = vve_.getEstimatedVelocity(); - float h = vve_.getEstimatedAltitude(); + const float gravity = 9.80665F; + const float velocity = vve_.getEstimatedVelocity(); + const float height = vve_.getEstimatedAltitude(); + + + //variables for analytical calculation + const float kVelocityEpsilon = 0.001F; + const float kVelocityScaleForAlpha = 150.0F; + const float kAlphaMin = 0.02F; + const float kAlphaMax = 0.25F; + const float kMinDragCoefficient = 0.00001F; + const float kApogeeFactor = 0.5F; + const float kBallisticDenominator = 2.0F; + //if the velocity is less than or equal to zero, the rocket has already reach apogee and the apogee is the current altitude - if (v <= 0.0F) + if (velocity <= 0.0F) { - predApogeeAlt_ = h; + predApogeeAlt_ = height; valid_ = true; return; } //gets the current acceleration of the rocket - float a = vve_.getInertialVerticalAcceleration(); + const float acceleration = vve_.getInertialVerticalAcceleration(); //calculates the measured drag coefficient - float kMeasured = -(a + g) / (v*v + 0.001F); +const float kMeasured = -(acceleration + gravity) / + (velocity * velocity + kVelocityEpsilon); if (kMeasured > 0.0F && kMeasured < 1.0F) { - float alpha = clamp(std::fabs(v) / 150.0F, 0.02F, 0.25F); + float alpha = clamp(std::fabs(velocity) / kVelocityScaleForAlpha, + kAlphaMin, + kAlphaMax); k_ = (1.0F - alpha) * k_ + alpha * kMeasured; } // Analytic apogee calculation float apogee = 0.0F; - if (k_ > 0.00001F) + if (k_ > kMinDragCoefficient) { - apogee = h + (0.5F / k_) * - logf((g + k_ * v * v) / g); + apogee = height + (kApogeeFactor / k_) * + logf((gravity + k_ * velocity * velocity) / gravity); } else { // fallback if drag unknown - apogee = h + (v * v) / (2.0F * g); + apogee = height + (velocity * velocity) / + (kBallisticDenominator * gravity); } predApogeeAlt_ = apogee; valid_ = true; } - // Simple getters bool ApogeePredictor::isPredictionValid() const { return valid_; } From cfb8d4855e238aff96dfec5c2950a91ddaf0ed9e Mon Sep 17 00:00:00 2001 From: Logan Rundle <154925928+LoganRundle-1@users.noreply.github.com> Date: Sun, 22 Feb 2026 15:22:18 -0500 Subject: [PATCH 5/5] changes k_ to currentDragCoefficient --- include/state_estimation/ApogeePredictor.h | 2 +- src/state_estimation/ApogeePredictor.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/state_estimation/ApogeePredictor.h b/include/state_estimation/ApogeePredictor.h index 1d4e112..11292ac 100644 --- a/include/state_estimation/ApogeePredictor.h +++ b/include/state_estimation/ApogeePredictor.h @@ -66,7 +66,7 @@ class ApogeePredictor { float tToApogee_; ///< Time until apogee (seconds) uint32_t predApogeeTs_; ///< Timestamp of predicted apogee (ms) float predApogeeAlt_;///< Predicted altitude at apogee (m) - float k_ = 0.0025F; ///< Drag coefficient + float currentDragCoefficient = 0.0025F; ///< Drag coefficient // Bookkeeping uint32_t lastTs_; ///< Last timestamp received diff --git a/src/state_estimation/ApogeePredictor.cpp b/src/state_estimation/ApogeePredictor.cpp index 02f1394..8782589 100644 --- a/src/state_estimation/ApogeePredictor.cpp +++ b/src/state_estimation/ApogeePredictor.cpp @@ -211,16 +211,16 @@ const float kMeasured = -(acceleration + gravity) / float alpha = clamp(std::fabs(velocity) / kVelocityScaleForAlpha, kAlphaMin, kAlphaMax); - k_ = (1.0F - alpha) * k_ + alpha * kMeasured; + currentDragCoefficient = (1.0F - alpha) * currentDragCoefficient + alpha * kMeasured; } // Analytic apogee calculation float apogee = 0.0F; - if (k_ > kMinDragCoefficient) + if (currentDragCoefficient > kMinDragCoefficient) { - apogee = height + (kApogeeFactor / k_) * - logf((gravity + k_ * velocity * velocity) / gravity); + apogee = height + (kApogeeFactor / currentDragCoefficient) * + logf((gravity + currentDragCoefficient * velocity * velocity) / gravity); } else { @@ -253,5 +253,5 @@ float ApogeePredictor::getFilteredDeceleration() const { } float ApogeePredictor::getdragCoefficient() const { - return k_; + return currentDragCoefficient; } \ No newline at end of file