Feature/apogee predictor

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

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;
+}