.. _program_listing_file_agents_gnss_attitude.cc:

Program Listing for File gnss_attitude.cc
=========================================

|exhale_lsh| :ref:`Return to documentation for file <file_agents_gnss_attitude.cc>` (``agents/gnss_attitude.cc``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   
   #include "lupnt/agents/gnss_attitude.h"
   
   #include "lupnt/agents/gnss_yaw_steering.h"
   #include "lupnt/numerics/math_utils.h"
   
   namespace lupnt {
   
     GnssAttitude::GnssAttitude(const Vec3& r_sat_eci, const Vec3& r_sun_eci) {
       Update(r_sat_eci, r_sun_eci);
     }
   
     void GnssAttitude::Compute(const Vec3& r_sat_eci, const Vec3& r_sun_eci, Vec3& ex, Vec3& ey,
                                Vec3& ez) {
       // GNSS satellite attitude frame (nadir / sun cross-track / along-track),
       // mirrors the attitude computation in `GNSSMeas.setup_measurements`:
       //   ez = normalize(-r_sat)                                (nadir)
       //   ey = normalize(cross(ez, normalize(r_sun - r_sat)))   (cross-track, toward Sun)
       //   ex = normalize(cross(ey, ez))                         (along-track, completes triad)
       ez = (-r_sat_eci).normalized();
       Vec3 e_to_sun = (r_sun_eci - r_sat_eci).normalized();
       ey = ez.cross(e_to_sun).normalized();
       ex = ey.cross(ez).normalized();
     }
   
     void GnssAttitude::ComputeFromYawAngle(const Vec3& r_sat_eci, const Vec3& v_sat_eci,
                                            Real yaw_angle, Vec3& ex, Vec3& ey, Vec3& ez) {
       // Orbital reference frame (the body frame at yaw angle phi = 0):
       //   ez     = normalize(-r_sat)              (nadir)
       //   ex_ref = normalize(v_sat)               (along-track)
       //   ey_ref = cross(ez, ex_ref)              (completes the right-handed triad)
       // The actual frame (ex, ey) is (ex_ref, ey_ref) rotated by `yaw_angle`
       // about the nadir axis ez:
       //   ex =  cos(phi) * ex_ref + sin(phi) * ey_ref
       //   ey = -sin(phi) * ex_ref + cos(phi) * ey_ref
       ez = (-r_sat_eci).normalized();
       Vec3 ex_ref = v_sat_eci.normalized();
       Vec3 ey_ref = ez.cross(ex_ref);
       Real c = cos(yaw_angle);
       Real s = sin(yaw_angle);
       ex = (c * ex_ref + s * ey_ref).normalized();
       ey = (-s * ex_ref + c * ey_ref).normalized();
     }
   
     void GnssAttitude::Compute(const Vec3& r_sat_eci, const Vec3& v_sat_eci, const Vec3& r_sun_eci,
                                Vec3& ex, Vec3& ey, Vec3& ez) {
       // Drive the attitude frame from the documented nominal yaw-steering law
       // (Kouba, 2009; Eq. 1 of Cheng et al., 2025): compute the Sun-elevation
       // angle `beta` and orbit angle `mu`, evaluate the nominal yaw angle
       // `phi = NominalYawAngle(beta, mu)`, and build the frame by rotating the
       // orbital reference frame about nadir by `phi` (see `ComputeFromYawAngle`).
       // This reproduces exactly the Sun-pointing frame of the geometric
       // `Compute(r_sat_eci, r_sun_eci, ...)` overload above (the nominal yaw law
       // is, by construction, the yaw angle that keeps the solar panels
       // Sun-pointing) -- verified numerically in `test_gnss_attitude.cc`.
       Real beta = GnssYawSteering::BetaAngle(r_sat_eci, v_sat_eci, r_sun_eci);
       Real mu = GnssYawSteering::OrbitAngle(r_sat_eci, v_sat_eci, r_sun_eci);
       Real phi = GnssYawSteering::NominalYawAngle(beta, mu);
       ComputeFromYawAngle(r_sat_eci, v_sat_eci, phi, ex, ey, ez);
     }
   
     void GnssAttitude::Update(const Vec3& r_sat_eci, const Vec3& r_sun_eci) {
       Compute(r_sat_eci, r_sun_eci, ex_, ey_, ez_);
     }
   
     void GnssAttitude::Update(const Vec3& r_sat_eci, const Vec3& v_sat_eci, const Vec3& r_sun_eci) {
       Compute(r_sat_eci, v_sat_eci, r_sun_eci, ex_, ey_, ez_);
     }
   
     Mat3 GnssAttitude::GetRotationMatrix() const {
       Mat3 R;
       R.col(0) = ex_;
       R.col(1) = ey_;
       R.col(2) = ez_;
       return R;
     }
   
     void GnssAttitude::GetAngles(const Vec3& u, Real& theta, Real& phi) const {
       phi = safe_acos(u.dot(ez_));
       theta = atan2(u.dot(ey_), u.dot(ex_));
     }
   
   }  // namespace lupnt