Q-Law orbit raising

[ChristopherRabotin]

High level description

I've tried this in the past, but failed. Gemini proposes a solution which I might as well try and debug.

Requirements

Implement Q-Law \o/

Test plans

The Q Law paper test cases are already in the code, so I'll just need to clone these for the new Q-Law implementation

Design

Document, discuss, and optionally upload design diagram into this section.

use nalgebra::Vector3;
use std::sync::Arc;

pub struct QLaw {
    pub objectives: Vec<Option<GuidanceObjective>>,
    pub weights: Vec<f64>,
    /// Scaling factors (tolerances) for each element, often x_i,tol in literature
    pub tolerances: Vec<f64>,
    pub max_eclipse_prct: Option<f64>,
}

impl GuidanceLaw for QLaw {
    fn achieved(&self, state: &Spacecraft) -> Result<bool, GuidanceError> {
        for obj in self.objectives.iter().flatten() {
            if !obj.assess_value(state.value(obj.parameter).context(GuidStateSnafu)?).0 {
                return Ok(false);
            }
        }
        Ok(true)
    }

    fn direction(&self, sc: &Spacecraft) -> Result<Vector3<f64>, GuidanceError> {
        if sc.mode() != GuidanceMode::Thrust {
            return Ok(Vector3::zeros());
        }

        let osc = sc.orbit;
        
        // Retrieve MEE elements: p, f, g, h, k, L
        // We'll use these to build the GVE matrix B(x)
        let p = osc.semi_parameter_km().context(GuidancePhysicsSnafu { action: "Q-Law" })?;
        let f = osc.mee_f().context(GuidancePhysicsSnafu { action: "Q-Law" })?;
        let g = osc.mee_g().context(GuidancePhysicsSnafu { action: "Q-Law" })?;
        let h = osc.mee_h().context(GuidancePhysicsSnafu { action: "Q-Law" })?;
        let k = osc.mee_k().context(GuidancePhysicsSnafu { action: "Q-Law" })?;
        let l = osc.true_longitude_rad().context(GuidancePhysicsSnafu { action: "Q-Law" })?;

        let (sin_l, cos_l) = l.sin_cos();
        let w = 1.0 + f * cos_l + g * sin_l;
        let s2 = 1.0 + h.powi(2) + k.powi(2);
        let sqrt_p_mu = (p / osc.gm_km_s2()).sqrt();

        // D is the direction vector: D = sum( dQ/dx_i * G_i )
        // where G_i is the row of the GVE matrix corresponding to element i
        let mut d_vector = Vector3::zeros();

        for (i, obj_opt) in self.objectives.iter().enumerate() {
            if let Some(obj) = obj_opt {
                let weight = self.weights[i];
                let tol = self.tolerances[i];
                let current_val = sc.value(obj.parameter).context(GuidStateSnafu)?;
                let target_val = obj.target; // Assuming GuidanceObjective stores a target
                
                // Partial of Q with respect to element x_i: 2 * W_i * (x_i - x_target) / tol^2
                let dq_dx = 2.0 * weight * (current_val - target_val) / tol.powi(2);

                // GVE components for the RTN frame (Radial, Transverse, Normal)
                let g_i = match obj.parameter {
                    StateParameter::Element(OrbitalElement::SemiParameter) => {
                        Vector3::new(0.0, 2.0 * p / w * sqrt_p_mu, 0.0)
                    }
                    StateParameter::Element(OrbitalElement::EquinoctialF) => {
                        Vector3::new(
                            sqrt_p_mu * sin_l,
                            sqrt_p_mu / w * ((w + 1.0) * cos_l + f),
                            -sqrt_p_mu * g / w * (h * sin_l - k * cos_l)
                        )
                    }
                    StateParameter::Element(OrbitalElement::EquinoctialG) => {
                        Vector3::new(
                            -sqrt_p_mu * cos_l,
                            sqrt_p_mu / w * ((w + 1.0) * sin_l + g),
                            sqrt_p_mu * f / w * (h * sin_l - k * cos_l)
                        )
                    }
                    StateParameter::Element(OrbitalElement::EquinoctialH) => {
                        Vector3::new(0.0, 0.0, sqrt_p_mu * s2 * cos_l / (2.0 * w))
                    }
                    StateParameter::Element(OrbitalElement::EquinoctialK) => {
                        Vector3::new(0.0, 0.0, sqrt_p_mu * s2 * sin_l / (2.0 * w))
                    }
                    _ => Vector3::zeros(),
                };

                d_vector += dq_dx * g_i;
            }
        }

        // To minimize Q, we thrust in the direction opposite to the gradient
        // Steering direction u = -D / |D|
        if d_vector.norm() > 0.0 {
            let steering_rtn = -d_vector.normalize();
            
            // Convert RTN to Inertial
            Ok(osc.dcm_from_rcn_to_inertial().context(GuidancePhysicsSnafu { action: "RTN to Inertial" })? * steering_rtn)
        } else {
            Ok(Vector3::zeros())
        }
    }

    fn throttle(&self, sc: &Spacecraft) -> Result<f64, GuidanceError> {
        // Q-law often includes an "effectivity" check. 
        // If the best dQ/dt is very small, it's better to coast.
        // For simplicity, we trigger full thrust if a direction is found.
        if sc.mode() == GuidanceMode::Thrust { Ok(1.0) } else { Ok(0.0) }
    }

    fn next(&self, sc: &mut Spacecraft, almanac: Arc<Almanac>) {
        // Reuse logic from previous implementations for eclipse and mode switching
    }
}

[ChristopherRabotin]

To implement the Effectivity Function () as Petropoulos defined it, we refine the throttle logic. The goal is to avoid "wasting" propellant when the spacecraft is at a position in its orbit where the thrust's contribution to reducing the proximity function is inefficient compared to the maximum possible contribution along that same orbit.

1. The Mathematical Logic

The effectivity of the thrust at any given moment is defined as:

Where:

• is the rate of change of with the current thrust vector.
• is the minimum possible (most negative) achievable at the current location in the orbit.
• The spacecraft only throttles up if exceeds a certain threshold (e.g., ).

---

2. Implementation in Nyx

We will update the QLaw struct to include an effectivity threshold and the logic to calculate the "best possible" at the current state to determine the throttle.

pub struct QLaw {
    pub objectives: Vec<Option<GuidanceObjective>>,
    pub weights: Vec<f64>,
    pub tolerances: Vec<f64>,
    /// The threshold for thrusting (0.0 to 1.0). 
    /// 0.0 means thrust always; 0.9 means only thrust at peak efficiency.
    pub eta_threshold: f64,
    pub max_eclipse_prct: Option<f64>,
}

impl GuidanceLaw for QLaw {
    // ... achieved() and direction() as implemented previously ...

    fn throttle(&self, sc: &Spacecraft) -> Result<f64, GuidanceError> {
        if sc.mode() != GuidanceMode::Thrust {
            return Ok(0.0);
        }

        let osc = sc.orbit;
        
        // 1. Calculate D vector (the gradient direction) as done in direction()
        // We need the magnitude of D because dot(D, unit_thrust) * thrust_acc = dQ_dt
        let d_vector = self.compute_d_vector(sc)?;
        let d_mag = d_vector.norm();

        if d_mag == 0.0 {
            return Ok(0.0);
        }

        // 2. Compute the best possible dQ_dt at this specific point in the orbit
        // Since we steer anti-parallel to D, dQ_dt_curr = -|D| * acceleration
        let acc = sc.max_thrust_acceleration(); // You'll need this from sc properties
        let dq_dt_curr = -d_mag * acc;

        // 3. To find the "Global Minimum" dQ_dt over the entire orbit (for effectivity),
        // Petropoulos suggests checking the dQ_dt at optimal points.
        // For a simplified real-time implementation, we compare against a 
        // filtered "best seen" or an analytical maximum.
        
        // Let's use the standard Q-Law effectivity: 
        // eta = (dQ_dt_curr) / (dQ_dt_min_absolute)
        // Here we'll use a pre-computed or estimated d_mag_max for the current orbit.
        let d_mag_max = self.estimate_max_grad_mag(osc);

        let eta = d_mag / d_mag_max;

        if eta >= self.eta_threshold {
            Ok(1.0)
        } else {
            Ok(0.0)
        }
    }
}

---

3. Key Refinement: Estimating the "Best" Efficiency

The hardest part of the effectivity function is knowing what the "best possible" is across the whole orbit without propagating it forward.

• The Heuristic Approach: Calculate the vector at discrete points (e.g., every 30° of true longitude) and find the maximum .
• The Analytical Approach: For specific transfers (like purely raising SMA), the max efficiency is always at perigee. For inclination, it's at the nodes.

Rust helper for the Heuristic:

impl QLaw {
    /// Estimates the maximum gradient magnitude over one orbital revolution
    fn estimate_max_grad_mag(&self, osc: Orbit) -> f64 {
        let mut max_d = 0.0;
        // Sample 12 points around the orbit
        for i in 0..12 {
            let l_sample = (i as f64) * 30.0f64.to_radians();
            let d_sample = self.compute_d_vector_at_longitude(osc, l_sample);
            if d_sample.norm() > max_d {
                max_d = d_sample.norm();
            }
        }
        max_d
    }
}

---

Implementation Notes for the Genetic Algorithm (GA)

When you run your GA to find the weights, you should include the eta_threshold as one of the genes.

• Low Threshold: Reaches the target faster (high thrust duty cycle) but uses more fuel.
• High Threshold: Takes much longer to reach the target (only thrusts at nodes/apses) but is extremely fuel-efficient.

By including this in Nyx, you've essentially built a "Mission Architect in a Box." You can throw any initial GTO at it, let the GA run for a few minutes to find the optimal weights and eta-cutoffs, and you'll have a flight-ready guidance law.