pronav_guidance.hpp

/****************************************************************************
 *
 * Copyright 2026 Auterion. All rights reserved.
 *
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Eigen>
#include <algorithm>
#include <cmath>
 
namespace auterion {
 
// ─── Guidance math primitives (pure functions) ────────────────────────────────
//
// These are building blocks for composing user-defined guidance strategies.
// They are intentionally small and orthogonal — each does exactly one thing.
// The strategy developer decides how and when to call them.
//
// No phase logic, no state machine, no assumptions about drone behaviour
// are encoded here. Hold, Midcourse, Terminal and any other concepts are
// defined entirely by the user in their own application.
 
inline Eigen::Vector3d proNav(double nav_gain, const Eigen::Vector3d& velocity,
                              const Eigen::Vector3d& los, const Eigen::Vector3d& los_rate,
                              double min_velocity = 15.0) {
    const double v = std::max(velocity.norm(), min_velocity);
    return -nav_gain * v * (los.normalized().cross(los_rate));
}
 
inline double altitudeHold(const Eigen::Vector3d& velocity, double gain = 4.0) {
    return -velocity.z() * gain;
}
 
inline Eigen::Vector3d slidingModeRobustifying(const Eigen::Vector3d& los,
                                               const Eigen::Vector3d& los_rate, double abias_max,
                                               double boundary_layer = 1e-3) {
    const Eigen::Vector3d s = los.normalized().cross(los_rate);
    return -abias_max * (s / std::max(s.norm(), boundary_layer));
}
 
}  // namespace auterion