Program Listing for File inertial_functions.hpp

↰ Return to documentation for file (src/navtk/inertial/inertial_functions.hpp)

#pragma once

#include <navtk/aspn.hpp>
#include <navtk/factory.hpp>
#include <navtk/inertial/AidingAltData.hpp>
#include <navtk/inspect.hpp>
#include <navtk/navutils/wgs84.hpp>
#include <navtk/tensors.hpp>

namespace navtk {
namespace inertial {
Vector3 calc_force_ned(const Matrix3& C_s_to_n, double dt, const Vector3& dth, const Vector3& dv);

Vector3 calc_force_ned(const aspn_xtensor::MeasurementPositionVelocityAttitude& pva1,
                       const aspn_xtensor::MeasurementPositionVelocityAttitude& pva2);

Vector3 calc_rot_rate(const Matrix3& C_s_to_n0,
                      double r_e,
                      double r_n,
                      double alt0,
                      double cos_l,
                      double dt,
                      const Vector3& dth,
                      double sin_l,
                      double tan_l,
                      const Vector3& v_ned0,
                      double omega = navutils::ROTATION_RATE);

Vector3 calc_rot_rate(const aspn_xtensor::MeasurementPositionVelocityAttitude& pva,
                      double dt,
                      const Vector3& dth);


// TODO: #690 Implement a more robust alternative which doesn't make a small angle assumption.
Vector3 calc_rot_rate(const aspn_xtensor::MeasurementPositionVelocityAttitude& pva1,
                      const aspn_xtensor::MeasurementPositionVelocityAttitude& pva2);

double apply_aiding_alt_accel(double r_zero,
                              Vector3* accel_vector,
                              AidingAltData* aiding_alt_data,
                              double alt0,
                              double dt,
                              const Vector3& g);

template <typename S1,
          typename S2,
          typename S3,
          typename S4,
          typename S5 = Vector,
          typename S6,
          typename S7,
          typename S8 = Vector,
          IfAllConditions<TENSORS_ARE_DIM<0, S1, S2, S3, S4, S6, S7>, TENSORS_ARE_DIM<1, S5, S8>>* =
              nullptr>
Vector3 calc_force_and_acceleration_offset(const S1& r_e,
                                           const S2& r_n,
                                           const S3& alt0,
                                           const S4& cos_l,
                                           const S5& g,
                                           const S6& sec_l,
                                           const S7& sin_l,
                                           const S8& v_ned0,
                                           double omega = navutils::ROTATION_RATE) {
    auto l_dot      = v_ned0(0) / (r_n + alt0);
    auto lambda_dot = v_ned0(1) * sec_l / (r_e + alt0);

    return {-v_ned0(1) * (2 * omega + lambda_dot) * sin_l + v_ned0(2) * l_dot + g(0),
            // Equation 3.77, pp 52, Titterton text
            v_ned0(0) * (2 * omega + lambda_dot) * sin_l +
                v_ned0(2) * (2 * omega + lambda_dot) * cos_l + g(1),
            // Equation 3.78, pp 52, Titterton text
            -v_ned0(1) * (2 * omega + lambda_dot) * cos_l - v_ned0(0) * l_dot + g(2)};
}

template <typename B1 = Vector,
          typename B2 = Vector,
          typename B3 = Vector,
          typename B4 = Vector,
          typename B5 = Matrix,
          typename B6 = Vector,
          typename B7 = Vector,
          typename B8 = Matrix,
          IfAllConditions<TENSORS_ARE_DIM<1, B1, B2, B3, B4, B6, B7>, TENSORS_ARE_DIM<2, B5, B8>>* =
              nullptr>
Matrix calc_force_and_acceleration_offset(const B1& r_e,
                                          const B2& r_n,
                                          const B3& alt0,
                                          const B4& cos_l,
                                          const B5& g,
                                          const B6& sec_l,
                                          const B7& sin_l,
                                          const B8& v_ned0,
                                          double omega = navutils::ROTATION_RATE) {

    const size_t N = r_e.shape()[0];

    Matrix offset = empty(N, 3);

    Scalar l_dot, lambda_dot;

    for (size_t i = 0; i < N; i++) {
        l_dot      = v_ned0(i, 0) / (r_n(i) + alt0(i));
        lambda_dot = v_ned0(i, 1) * sec_l(i) / (r_e(i) + alt0(i));

        offset(i, 0) =
            -v_ned0(i, 1) * (2 * omega + lambda_dot) * sin_l(i) + v_ned0(i, 2) * l_dot + g(i, 0);
        // Equation 3.77, pp 52, Titterton text
        offset(i, 1) = v_ned0(i, 0) * (2 * omega + lambda_dot) * sin_l(i) +
                       v_ned0(i, 2) * (2 * omega + lambda_dot) * cos_l(i) + g(i, 1);
        // Equation 3.78, pp 52, Titterton text
        offset(i, 2) =
            -v_ned0(i, 1) * (2 * omega + lambda_dot) * cos_l(i) - v_ned0(i, 0) * l_dot + g(i, 2);
    }

    return offset;
}

}  // namespace inertial
}  // namespace navtk