FrameKinematics.hpp

Go to the documentation of this file.

// Vulcan Coordinate Transforms
// Velocity transforms and non-inertial accelerations for ECEF/ECI
#pragma once
 
#include <vulcan/coordinates/EarthModel.hpp>
#include <vulcan/coordinates/FramePrimitives.hpp>
#include <vulcan/core/VulcanTypes.hpp>
 
#include <janus/math/Linalg.hpp>
 
namespace vulcan {
 
// =============================================================================
// Velocity Transforms
// =============================================================================

template <typename Scalar>
Vec3<Scalar> velocity_ecef_to_eci(const Vec3<Scalar> &v_ecef,
                                  const Vec3<Scalar> &r_ecef,
                                  const CoordinateFrame<Scalar> &eci,
                                  const EarthModel &m = EarthModel::WGS84()) {
    // Earth's angular velocity vector in ECEF: [0, 0, omega]
    Vec3<Scalar> omega_ecef;
    omega_ecef << Scalar(0), Scalar(0), Scalar(m.omega);
 
    // omega x r gives velocity contribution from rotation
    Vec3<Scalar> omega_cross_r = janus::cross(omega_ecef, r_ecef);
 
    // v_ecef_inertial = v_ecef + omega x r (velocity of ECEF point in inertial
    // frame)
    Vec3<Scalar> v_ecef_inertial = v_ecef + omega_cross_r;
 
    // Transform to ECI
    return eci.from_ecef(v_ecef_inertial);
}

template <typename Scalar>
Vec3<Scalar> velocity_eci_to_ecef(const Vec3<Scalar> &v_eci,
                                  const Vec3<Scalar> &r_ecef,
                                  const CoordinateFrame<Scalar> &eci,
                                  const EarthModel &m = EarthModel::WGS84()) {
    // Transform v_eci to ECEF
    Vec3<Scalar> v_ecef_inertial = eci.to_ecef(v_eci);
 
    // Earth's angular velocity in ECEF
    Vec3<Scalar> omega_ecef;
    omega_ecef << Scalar(0), Scalar(0), Scalar(m.omega);
 
    // Subtract omega x r
    Vec3<Scalar> omega_cross_r = janus::cross(omega_ecef, r_ecef);
 
    return v_ecef_inertial - omega_cross_r;
}
 
// =============================================================================
// Non-Inertial Accelerations
// =============================================================================

template <typename Scalar>
Vec3<Scalar> coriolis_centrifugal(const Vec3<Scalar> &r_ecef,
                                  const Vec3<Scalar> &v_ecef,
                                  const EarthModel &m = EarthModel::WGS84()) {
    // Earth's angular velocity vector in ECEF: [0, 0, omega]
    Vec3<Scalar> omega;
    omega << Scalar(0), Scalar(0), Scalar(m.omega);
 
    // Coriolis: -2(omega x v)
    Vec3<Scalar> coriolis = Scalar(-2) * janus::cross(omega, v_ecef);
 
    // Centrifugal: -omega x (omega x r)
    Vec3<Scalar> omega_cross_r = janus::cross(omega, r_ecef);
    Vec3<Scalar> centrifugal = -janus::cross(omega, omega_cross_r);
 
    return coriolis + centrifugal;
}

template <typename Scalar>
Vec3<Scalar> coriolis_acceleration(const Vec3<Scalar> &v_ecef,
                                   const EarthModel &m = EarthModel::WGS84()) {
    Vec3<Scalar> omega;
    omega << Scalar(0), Scalar(0), Scalar(m.omega);
 
    return Scalar(-2) * janus::cross(omega, v_ecef);
}

template <typename Scalar>
Vec3<Scalar>
centrifugal_acceleration(const Vec3<Scalar> &r_ecef,
                         const EarthModel &m = EarthModel::WGS84()) {
    Vec3<Scalar> omega;
    omega << Scalar(0), Scalar(0), Scalar(m.omega);
 
    Vec3<Scalar> omega_cross_r = janus::cross(omega, r_ecef);
    return -janus::cross(omega, omega_cross_r);
}
 
// =============================================================================
// Relative Position/Velocity
// =============================================================================

template <typename Scalar>
Vec3<Scalar> relative_position(const Vec3<Scalar> &r_target,
                               const Vec3<Scalar> &r_observer) {
    return r_target - r_observer;
}

template <typename Scalar>
Scalar range(const Vec3<Scalar> &r1, const Vec3<Scalar> &r2) {
    return janus::norm(r1 - r2);
}

template <typename Scalar>
Scalar range_rate(const Vec3<Scalar> &r_target, const Vec3<Scalar> &v_target,
                  const Vec3<Scalar> &r_observer,
                  const Vec3<Scalar> &v_observer) {
    Vec3<Scalar> r_rel = r_target - r_observer;
    Vec3<Scalar> v_rel = v_target - v_observer;
 
    Scalar r_mag = janus::norm(r_rel);
    Scalar eps = Scalar(1e-10);
    Scalar is_zero = r_mag < eps;
 
    // Range rate = relative velocity dot unit line-of-sight
    Scalar rdot = janus::dot(r_rel, v_rel) / r_mag;
 
    // Handle zero range case
    return janus::where(is_zero, Scalar(0), rdot);
}
 
// =============================================================================
// State Propagation Helpers
// =============================================================================

template <typename Scalar>
Vec3<Scalar> omega_ned_wrt_ecef(const Vec3<Scalar> &v_ned, Scalar lat,
                                Scalar alt,
                                const EarthModel &m = EarthModel::WGS84()) {
    Scalar sin_lat = janus::sin(lat);
    Scalar cos_lat = janus::cos(lat);
 
    // Radius of curvature in meridian
    Scalar denom = janus::sqrt(Scalar(1) - Scalar(m.e2) * sin_lat * sin_lat);
    Scalar R_n =
        Scalar(m.a) * (Scalar(1) - Scalar(m.e2)) / (denom * denom * denom);
    R_n = R_n + alt;
 
    // Radius of curvature in prime vertical
    Scalar R_e = Scalar(m.a) / denom + alt;
 
    // Angular velocity of NED wrt ECEF in NED coordinates
    Vec3<Scalar> omega;
    omega << -v_ned(0) / R_n, v_ned(1) / R_e, v_ned(1) * janus::tan(lat) / R_e;
 
    return omega;
}

template <typename Scalar>
Vec3<Scalar> omega_ned_wrt_eci(const Vec3<Scalar> &v_ned, Scalar lat,
                               Scalar alt,
                               const EarthModel &m = EarthModel::WGS84()) {
    // Angular velocity of NED wrt ECEF
    Vec3<Scalar> omega_ned_ecef = omega_ned_wrt_ecef(v_ned, lat, alt, m);
 
    // Earth's angular velocity in NED: [omega*cos(lat), 0, -omega*sin(lat)]
    Scalar sin_lat = janus::sin(lat);
    Scalar cos_lat = janus::cos(lat);
 
    Vec3<Scalar> omega_earth_ned;
    omega_earth_ned << Scalar(m.omega) * cos_lat, Scalar(0),
        -Scalar(m.omega) * sin_lat;
 
    return omega_ned_ecef + omega_earth_ned;
}
 
} // namespace vulcan