.. _program_listing_file_applications_ephemeris_ephemeris_basis.h:

Program Listing for File ephemeris_basis.h
==========================================

|exhale_lsh| :ref:`Return to documentation for file <file_applications_ephemeris_ephemeris_basis.h>` (``applications/ephemeris/ephemeris_basis.h``)

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

.. code-block:: cpp

   #pragma once
   
   #include <algorithm>
   #include <cmath>
   
   #include "lupnt/applications/ephemeris/lunanet_ephemeris.h"
   #include "lupnt/core/constants.h"
   #include "lupnt/core/definitions.h"
   
   namespace lupnt {
   
     inline MatXd ChebyshevBasis(const VecXd& t_k, double t_fit, int order) {
       const int n = static_cast<int>(t_k.size());
       MatXd T(n, order + 1);
       T.col(0).setOnes();
       if (order == 0) return T;
       VecXd z = (2.0 / t_fit) * t_k;
       T.col(1) = z;
       for (int j = 2; j <= order; ++j) {
         T.col(j) = 2.0 * z.array() * T.col(j - 1).array() - T.col(j - 2).array();
       }
       return T;
     }
   
     inline MatXd ChebyshevBasisDt(const VecXd& t_k, double t_fit, int order) {
       const int n = static_cast<int>(t_k.size());
       MatXd T = ChebyshevBasis(t_k, t_fit, order);
       MatXd Tdot(n, order + 1);
       Tdot.col(0).setZero();
       if (order == 0) return Tdot;
       const double dzdt = 2.0 / t_fit;
       VecXd z = (2.0 / t_fit) * t_k;
       Tdot.col(1).setConstant(dzdt);
       for (int j = 2; j <= order; ++j) {
         Tdot.col(j) = 2.0 * z.array() * Tdot.col(j - 1).array() + 2.0 * T.col(j - 1).array() * dzdt
                       - Tdot.col(j - 2).array();
       }
       return Tdot;
     }
   
     inline VecXd UnwrapAngles(const VecXd& angles) {
       VecXd out = angles;
       for (int i = 1; i < out.size(); ++i) {
         while (out(i) - out(i - 1) > PI) out(i) -= 2.0 * PI;
         while (out(i) - out(i - 1) < -PI) out(i) += 2.0 * PI;
       }
       return out;
     }
   
     inline VecXd CumTrapz(const VecXd& y, const VecXd& t) {
       VecXd out = VecXd::Zero(y.size());
       for (int i = 1; i < y.size(); ++i) {
         out(i) = out(i - 1) + 0.5 * (y(i) + y(i - 1)) * (t(i) - t(i - 1));
       }
       return out;
     }
   
     inline Mat3d RtnMatrix(const Vec6d& rv) {
       const Vec3d r = rv.head<3>();
       const Vec3d v = rv.tail<3>();
       const Vec3d R = r.normalized();
       const Vec3d N = (r.cross(v)).normalized();
       const Vec3d T = N.cross(R);
       Mat3d M;
       M.row(0) = R.transpose();
       M.row(1) = T.transpose();
       M.row(2) = N.transpose();
       return M;
     }
   
     inline double Percentile95(VecXd v) {
       std::sort(v.data(), v.data() + v.size());
       int idx = std::max(0, static_cast<int>(std::ceil(0.95 * v.size())) - 1);
       idx = std::min<int>(idx, static_cast<int>(v.size()) - 1);
       return v(idx);
     }
   
     inline EphemerisFitErrorStats ComputeFitErrorStats(const MatXd& rv_fit, const MatXd& rv_ref) {
       const int n = static_cast<int>(rv_ref.rows());
       MatXd diff = rv_fit - rv_ref;
       MatXd diff_rtn(n, 6);
       for (int i = 0; i < n; ++i) {
         const Vec6d rv_row = rv_ref.row(i).transpose();
         const Mat3d M = RtnMatrix(rv_row);
         diff_rtn.row(i).head<3>() = (M * diff.row(i).head<3>().transpose()).transpose();
         diff_rtn.row(i).tail<3>() = (M * diff.row(i).tail<3>().transpose()).transpose();
       }
   
       EphemerisFitErrorStats stats;
       for (int d = 0; d < 3; ++d) {
         stats.rms_pos_m(d) = std::sqrt(diff_rtn.col(d).array().square().mean());
         stats.rms_vel_mps(d) = std::sqrt(diff_rtn.col(3 + d).array().square().mean());
         stats.p95_pos_m(d) = Percentile95(diff_rtn.col(d).array().abs().matrix());
         stats.p95_vel_mps(d) = Percentile95(diff_rtn.col(3 + d).array().abs().matrix());
       }
       stats.rms_pos_m(3) = std::sqrt(diff_rtn.leftCols(3).array().square().sum() / n);
       stats.rms_vel_mps(3) = std::sqrt(diff_rtn.rightCols(3).array().square().sum() / n);
   
       VecXd pos_norm(n), vel_norm(n);
       for (int i = 0; i < n; ++i) {
         pos_norm(i) = diff_rtn.row(i).head<3>().norm();
         vel_norm(i) = diff_rtn.row(i).tail<3>().norm();
       }
       stats.p95_pos_m(3) = Percentile95(pos_norm);
       stats.p95_vel_mps(3) = Percentile95(vel_norm);
   
       return stats;
     }
   
   }  // namespace lupnt