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,
typename S6,
typename S7,
typename S8,
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 v_ned0_vec = to_vec(v_ned0);
auto g_vec = to_vec(g);
auto l_dot = v_ned0_vec(0) / (r_n + alt0);
auto lambda_dot = v_ned0_vec(1) * sec_l / (r_e + alt0);
return {-v_ned0_vec(1) * (2 * omega + lambda_dot) * sin_l + v_ned0_vec(2) * l_dot + g_vec(0),
// Equation 3.77, pp 52, Titterton text
v_ned0_vec(0) * (2 * omega + lambda_dot) * sin_l +
v_ned0_vec(2) * (2 * omega + lambda_dot) * cos_l + g_vec(1),
// Equation 3.78, pp 52, Titterton text
-v_ned0_vec(1) * (2 * omega + lambda_dot) * cos_l - v_ned0_vec(0) * l_dot + g_vec(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,
IfAnyConditions<TENSORS_HAVE_MAX_DIM<1, B1, B2, B3, B4, B6, B7>,
TENSORS_HAVE_MAX_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) {
auto r_e_vec = to_vec(r_e);
auto r_n_vec = to_vec(r_n);
auto alt0_vec = to_vec(alt0);
auto cos_l_vec = to_vec(cos_l);
auto g_mat = to_matrix(g);
auto sec_l_vec = to_vec(sec_l);
auto sin_l_vec = to_vec(sin_l);
auto v_ned0_mat = to_matrix(v_ned0);
const size_t N = std::max({r_e_vec.shape()[0],
r_n_vec.shape()[0],
alt0_vec.shape()[0],
cos_l_vec.shape()[0],
g_mat.shape()[0],
sec_l_vec.shape()[0],
sin_l_vec.shape()[0],
v_ned0_mat.shape()[0]});
Matrix offset = empty(N, 3);
Scalar l_dot, lambda_dot;
for (size_t i = 0; i < N; i++) {
l_dot = v_ned0_mat(i, 0) / (r_n_vec(i) + alt0_vec(i));
lambda_dot = v_ned0_mat(i, 1) * sec_l_vec(i) / (r_e_vec(i) + alt0_vec(i));
offset(i, 0) = -v_ned0_mat(i, 1) * (2 * omega + lambda_dot) * sin_l_vec(i) +
v_ned0_mat(i, 2) * l_dot + g_mat(i, 0);
// Equation 3.77, pp 52, Titterton text
offset(i, 1) = v_ned0_mat(i, 0) * (2 * omega + lambda_dot) * sin_l_vec(i) +
v_ned0_mat(i, 2) * (2 * omega + lambda_dot) * cos_l_vec(i) + g_mat(i, 1);
// Equation 3.78, pp 52, Titterton text
offset(i, 2) = -v_ned0_mat(i, 1) * (2 * omega + lambda_dot) * cos_l_vec(i) -
v_ned0_mat(i, 0) * l_dot + g_mat(i, 2);
}
return offset;
}
} // namespace inertial
} // namespace navtk