VonKarmanTurbulence.hpp

Go to the documentation of this file.

// von Kármán Turbulence Model
// Higher-order approximate forming filters (MIL-F-8785C / MIL-HDBK-1797)
#pragma once
 
#include <array>
#include <cmath>
#include <vulcan/wind/WindTypes.hpp>
 
namespace vulcan::von_karman {
 
// ============================================================================
// Physical Constants
// ============================================================================

inline constexpr double SPECTRAL_CONSTANT = 1.339;
 
// ============================================================================
// Power Spectral Density Functions
// ============================================================================

template <typename Scalar>
Scalar psd_longitudinal(const Scalar &omega, double sigma_u, double L_u) {
    Scalar L_omega = SPECTRAL_CONSTANT * L_u * omega;
    Scalar denom = Scalar(1) + L_omega * L_omega;
    // (1 + x)^(5/6) = (1 + x)^(5/6)
    return sigma_u * sigma_u * (2.0 * L_u / M_PI) /
           janus::pow(denom, 5.0 / 6.0);
}

template <typename Scalar>
Scalar psd_lateral(const Scalar &omega, double sigma, double L) {
    Scalar L_omega = SPECTRAL_CONSTANT * L * omega;
    Scalar L_omega_sq = L_omega * L_omega;
    Scalar numer = Scalar(1) + Scalar(8.0 / 3.0) * L_omega_sq;
    Scalar denom = Scalar(1) + L_omega_sq;
    return sigma * sigma * (L / M_PI) * numer / janus::pow(denom, 11.0 / 6.0);
}
 
// ============================================================================
// Forming Filter State (Higher Order)
// ============================================================================

template <typename Scalar> struct FilterState {
    std::array<Scalar, 3> x_u; 
    std::array<Scalar, 4> x_v; 
    std::array<Scalar, 4> x_w; 
};

template <typename Scalar> FilterState<Scalar> init_state() {
    FilterState<Scalar> state;
    for (auto &s : state.x_u)
        s = Scalar(0);
    for (auto &s : state.x_v)
        s = Scalar(0);
    for (auto &s : state.x_w)
        s = Scalar(0);
    return state;
}
 
// ============================================================================
// Filter Coefficients
// ============================================================================

struct FilterCoeffs {
    // Longitudinal (3rd order discrete system)
    std::array<std::array<double, 3>, 3> A_u; 
    std::array<double, 3> B_u;                
    std::array<double, 3> C_u;                
    double D_u;                               
 
    // Lateral (4th order discrete system)
    std::array<std::array<double, 4>, 4> A_v;
    std::array<double, 4> B_v;
    std::array<double, 4> C_v;
    double D_v;
 
    // Vertical (4th order discrete system)
    std::array<std::array<double, 4>, 4> A_w;
    std::array<double, 4> B_w;
    std::array<double, 4> C_w;
    double D_w;
};

inline FilterCoeffs
compute_filter_coeffs(const wind::TurbulenceParams<double> &params,
                      double airspeed, double dt) {
    FilterCoeffs coeffs;
 
    // Longitudinal filter (3rd order approximation)
    // Uses cascade of 3 first-order sections with varied time constants
    // to approximate the 5/6 roll-off
    double tau_u = SPECTRAL_CONSTANT * params.L_u / airspeed;
 
    // Time constants for cascade approximation
    double tau1 = tau_u * 1.0;
    double tau2 = tau_u * 0.618; // Golden ratio provides good spread
    double tau3 = tau_u * 0.382;
 
    // Discretize using exponential mapping (exact for first-order)
    double a1 = std::exp(-dt / tau1);
    double a2 = std::exp(-dt / tau2);
    double a3 = std::exp(-dt / tau3);
 
    // State transition matrix (diagonal for cascade)
    coeffs.A_u = {{{a1, 0, 0}, {0, a2, 0}, {0, 0, a3}}};
 
    // Input vector
    double k_u =
        params.sigma_u * std::sqrt(2.0 * params.L_u / (M_PI * airspeed));
    coeffs.B_u = {std::sqrt(1 - a1 * a1) * k_u, std::sqrt(1 - a2 * a2) * 0.5,
                  std::sqrt(1 - a3 * a3) * 0.25};
 
    // Output vector (weighted sum)
    coeffs.C_u = {0.6, 0.3, 0.1};
    coeffs.D_u = 0.0;
 
    // Lateral filter (4th order approximation)
    double tau_v = SPECTRAL_CONSTANT * params.L_v / airspeed;
    double tau_v1 = tau_v * 1.0;
    double tau_v2 = tau_v * 0.75;
    double tau_v3 = tau_v * 0.5;
    double tau_v4 = tau_v * 0.25;
 
    double av1 = std::exp(-dt / tau_v1);
    double av2 = std::exp(-dt / tau_v2);
    double av3 = std::exp(-dt / tau_v3);
    double av4 = std::exp(-dt / tau_v4);
 
    coeffs.A_v = {
        {{av1, 0, 0, 0}, {0, av2, 0, 0}, {0, 0, av3, 0}, {0, 0, 0, av4}}};
 
    double k_v = params.sigma_v * std::sqrt(params.L_v / (M_PI * airspeed));
    coeffs.B_v = {std::sqrt(1 - av1 * av1) * k_v,
                  std::sqrt(1 - av2 * av2) * k_v * 0.707,
                  std::sqrt(1 - av3 * av3) * k_v * 0.5,
                  std::sqrt(1 - av4 * av4) * k_v * 0.25};
 
    // Output weights for proper spectrum shaping
    coeffs.C_v = {0.4, 0.35, 0.2, 0.05};
    coeffs.D_v = 0.0;
 
    // Vertical filter (same structure as lateral, different parameters)
    double tau_w = SPECTRAL_CONSTANT * params.L_w / airspeed;
    double tau_w1 = tau_w * 1.0;
    double tau_w2 = tau_w * 0.75;
    double tau_w3 = tau_w * 0.5;
    double tau_w4 = tau_w * 0.25;
 
    double aw1 = std::exp(-dt / tau_w1);
    double aw2 = std::exp(-dt / tau_w2);
    double aw3 = std::exp(-dt / tau_w3);
    double aw4 = std::exp(-dt / tau_w4);
 
    coeffs.A_w = {
        {{aw1, 0, 0, 0}, {0, aw2, 0, 0}, {0, 0, aw3, 0}, {0, 0, 0, aw4}}};
 
    double k_w = params.sigma_w * std::sqrt(params.L_w / (M_PI * airspeed));
    coeffs.B_w = {std::sqrt(1 - aw1 * aw1) * k_w,
                  std::sqrt(1 - aw2 * aw2) * k_w * 0.707,
                  std::sqrt(1 - aw3 * aw3) * k_w * 0.5,
                  std::sqrt(1 - aw4 * aw4) * k_w * 0.25};
 
    coeffs.C_w = {0.4, 0.35, 0.2, 0.05};
    coeffs.D_w = 0.0;
 
    return coeffs;
}
 
// ============================================================================
// Filter Step Function
// ============================================================================

template <typename Scalar>
wind::GustVelocity<Scalar>
step(FilterState<Scalar> &state, const FilterCoeffs &coeffs,
     const Scalar &noise_u, const Scalar &noise_v, const Scalar &noise_w) {
    // Longitudinal (3rd order cascade)
    std::array<Scalar, 3> new_x_u;
    Scalar u_g = Scalar(0);
    for (size_t i = 0; i < 3; ++i) {
        new_x_u[i] = coeffs.A_u[i][i] * state.x_u[i] + coeffs.B_u[i] * noise_u;
        u_g = u_g + coeffs.C_u[i] * new_x_u[i];
    }
    state.x_u = new_x_u;
 
    // Lateral (4th order cascade)
    std::array<Scalar, 4> new_x_v;
    Scalar v_g = Scalar(0);
    for (size_t i = 0; i < 4; ++i) {
        new_x_v[i] = coeffs.A_v[i][i] * state.x_v[i] + coeffs.B_v[i] * noise_v;
        v_g = v_g + coeffs.C_v[i] * new_x_v[i];
    }
    state.x_v = new_x_v;
 
    // Vertical (4th order cascade)
    std::array<Scalar, 4> new_x_w;
    Scalar w_g = Scalar(0);
    for (size_t i = 0; i < 4; ++i) {
        new_x_w[i] = coeffs.A_w[i][i] * state.x_w[i] + coeffs.B_w[i] * noise_w;
        w_g = w_g + coeffs.C_w[i] * new_x_w[i];
    }
    state.x_w = new_x_w;
 
    return wind::GustVelocity<Scalar>{.u_g = u_g, .v_g = v_g, .w_g = w_g};
}
 
// ============================================================================
// Convenience Functions
// ============================================================================

inline FilterCoeffs mil_spec_coeffs(double altitude,
                                    wind::TurbulenceSeverity severity,
                                    double airspeed, double dt) {
    wind::TurbulenceParams<double> params =
        wind::mil_spec_params(altitude, severity);
    return compute_filter_coeffs(params, airspeed, dt);
}
 
} // namespace vulcan::von_karman