SecondOrder.hpp

Go to the documentation of this file.

#pragma once
 
#include <janus/janus.hpp>
 
namespace vulcan::tf {

template <typename Scalar> struct SecondOrderCoeffs {
    Eigen::Matrix<Scalar, 2, 2> A; 
    Eigen::Matrix<Scalar, 2, 1> B; 
    Eigen::Matrix<Scalar, 1, 2> C; 
    Scalar D;                      
    double omega_n;                
    double zeta;                   
    double K;                      
    double dt;                     
};

template <typename Scalar>
SecondOrderCoeffs<Scalar> second_order(double omega_n, double zeta, double K,
                                       double dt) {
    SecondOrderCoeffs<Scalar> c;
    c.omega_n = omega_n;
    c.zeta = zeta;
    c.K = K;
    c.dt = dt;
 
    double wn = omega_n;
    double z = zeta;
    double wn2 = wn * wn;
 
    if (z < 1.0) {
        // Underdamped
        double wd = wn * std::sqrt(1.0 - z * z);
        double sigma = z * wn;
        double e_sigma = std::exp(-sigma * dt);
        double cos_wd = std::cos(wd * dt);
        double sin_wd = std::sin(wd * dt);
 
        c.A(0, 0) =
            static_cast<Scalar>(e_sigma * (cos_wd + (sigma / wd) * sin_wd));
        c.A(0, 1) = static_cast<Scalar>(e_sigma * sin_wd / wd);
        c.A(1, 0) = static_cast<Scalar>(-e_sigma * wn2 * sin_wd / wd);
        c.A(1, 1) =
            static_cast<Scalar>(e_sigma * (cos_wd - (sigma / wd) * sin_wd));
 
        double b0 = K * (1.0 - e_sigma * (cos_wd + (sigma / wd) * sin_wd));
        double b1 = K * e_sigma * wn2 * sin_wd / wd;
        c.B(0) = static_cast<Scalar>(b0);
        c.B(1) = static_cast<Scalar>(b1);
 
    } else if (z > 1.0) {
        // Overdamped
        double sqrtTerm = wn * std::sqrt(z * z - 1.0);
        double p1 = -z * wn + sqrtTerm;
        double p2 = -z * wn - sqrtTerm;
        double e1 = std::exp(p1 * dt);
        double e2 = std::exp(p2 * dt);
        double dp = p1 - p2;
 
        c.A(0, 0) = static_cast<Scalar>((p1 * e2 - p2 * e1) / dp);
        c.A(0, 1) = static_cast<Scalar>((e1 - e2) / dp);
        c.A(1, 0) = static_cast<Scalar>(p1 * p2 * (e2 - e1) / dp);
        c.A(1, 1) = static_cast<Scalar>((p1 * e1 - p2 * e2) / dp);
 
        double b0 = K * (1.0 - (p1 * e2 - p2 * e1) / dp);
        double b1 = K * p1 * p2 * (e1 - e2) / dp;
        c.B(0) = static_cast<Scalar>(b0);
        c.B(1) = static_cast<Scalar>(b1);
 
    } else {
        // Critically damped
        double e_wn = std::exp(-wn * dt);
 
        c.A(0, 0) = static_cast<Scalar>(e_wn * (1.0 + wn * dt));
        c.A(0, 1) = static_cast<Scalar>(e_wn * dt);
        c.A(1, 0) = static_cast<Scalar>(-e_wn * wn * wn * dt);
        c.A(1, 1) = static_cast<Scalar>(e_wn * (1.0 - wn * dt));
 
        double b0 = K * (1.0 - e_wn * (1.0 + wn * dt));
        double b1 = K * e_wn * wn * wn * dt;
        c.B(0) = static_cast<Scalar>(b0);
        c.B(1) = static_cast<Scalar>(b1);
    }
 
    c.C(0, 0) = static_cast<Scalar>(1.0);
    c.C(0, 1) = static_cast<Scalar>(0.0);
    c.D = static_cast<Scalar>(0.0);
 
    return c;
}

template <typename Scalar>
Eigen::Matrix<Scalar, 2, 1>
second_order_step(const SecondOrderCoeffs<Scalar> &coeffs,
                  const Eigen::Matrix<Scalar, 2, 1> &state,
                  const Scalar &input) {
    return coeffs.A * state + coeffs.B * input;
}

template <typename Scalar>
Scalar second_order_output(const SecondOrderCoeffs<Scalar> &coeffs,
                           const Eigen::Matrix<Scalar, 2, 1> &state,
                           const Scalar &input) {
    return (coeffs.C * state)(0) + coeffs.D * input;
}

inline std::tuple<double, double, double>
second_order_characteristics(double omega_n, double zeta) {
    double tr = (1.8 - 0.6 * zeta) / omega_n;
    double ts = 4.0 / (zeta * omega_n);
    double Mp = 0.0;
    if (zeta < 1.0 && zeta > 0.0) {
        Mp = 100.0 * std::exp(-zeta * M_PI / std::sqrt(1.0 - zeta * zeta));
    }
    return {tr, ts, Mp};
}
 
} // namespace vulcan::tf