diff --git a/include/state_estimation/ApogeePredictor.h b/include/state_estimation/ApogeePredictor.h index 3067a3b..11292ac 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 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 0ef0cb2..8782589 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, @@ -170,6 +173,65 @@ void ApogeePredictor::poly_update() { } +void ApogeePredictor::analytic_update() +{ + //gets the gravity constant and the current velocity and altitude of the rocket + 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 (velocity <= 0.0F) + { + predApogeeAlt_ = height; + valid_ = true; + return; + } + + //gets the current acceleration of the rocket + const float acceleration = vve_.getInertialVerticalAcceleration(); + + //calculates the measured drag coefficient +const float kMeasured = -(acceleration + gravity) / + (velocity * velocity + kVelocityEpsilon); + + if (kMeasured > 0.0F && kMeasured < 1.0F) + { + float alpha = clamp(std::fabs(velocity) / kVelocityScaleForAlpha, + kAlphaMin, + kAlphaMax); + currentDragCoefficient = (1.0F - alpha) * currentDragCoefficient + alpha * kMeasured; + } + + // Analytic apogee calculation + float apogee = 0.0F; + + if (currentDragCoefficient > kMinDragCoefficient) + { + apogee = height + (kApogeeFactor / currentDragCoefficient) * + logf((gravity + currentDragCoefficient * velocity * velocity) / gravity); + } + else + { + // fallback if drag unknown + apogee = height + (velocity * velocity) / + (kBallisticDenominator * gravity); + } + + predApogeeAlt_ = apogee; + valid_ = true; +} // Simple getters bool ApogeePredictor::isPredictionValid() const { return valid_; } @@ -189,3 +251,7 @@ float ApogeePredictor::getPredictedApogeeAltitude_m() const { float ApogeePredictor::getFilteredDeceleration() const { return filteredDecel_; } + +float ApogeePredictor::getdragCoefficient() const { + return currentDragCoefficient; +} \ No newline at end of file