Program Listing for File navigation.hpp
↰ Return to documentation for file (src/navtk/navutils/navigation.hpp)
#pragma once
#include <memory>
#include <utility>
#include <navtk/factory.hpp>
#include <navtk/inspect.hpp>
#include <navtk/navutils/math.hpp>
#include <navtk/navutils/wgs84.hpp>
#include <navtk/tensors.hpp>
namespace navtk {
namespace navutils {
using std::size_t;
template <typename S, IfTensorOfDim<S, 0>* = nullptr>
double meridian_radius(const S& latitude) {
auto sin_lat = sin(latitude);
return SEMI_MAJOR_RADIUS * (1 - ECCENTRICITY_SQUARED) /
pow(1 - ECCENTRICITY_SQUARED * sin_lat * sin_lat, 1.5);
}
template <typename B, IfTensorOfDim<B, 1>* = nullptr>
Vector meridian_radius(const B& latitude) {
auto sin_lat = xt::sin(latitude);
return SEMI_MAJOR_RADIUS * (1 - ECCENTRICITY_SQUARED) /
xt::pow(1 - ECCENTRICITY_SQUARED * sin_lat * sin_lat, 1.5);
}
template <typename S, IfTensorOfDim<S, 0>* = nullptr>
double transverse_radius(const S& latitude) {
auto sin_lat = sin(latitude);
return SEMI_MAJOR_RADIUS / pow(1 - ECCENTRICITY_SQUARED * sin_lat * sin_lat, 0.5);
}
template <typename B, IfTensorOfDim<B, 1>* = nullptr>
Vector transverse_radius(const B& latitude) {
auto sin_lat = xt::sin(latitude);
return SEMI_MAJOR_RADIUS / xt::pow(1 - ECCENTRICITY_SQUARED * sin_lat * sin_lat, 0.5);
}
Matrix3 axis_angle_to_dcm(const Vector3& axis, double angle);
Matrix calc_van_loan(const Matrix& F, const Matrix& G, const Matrix& Q, double dt);
Matrix3 correct_dcm_with_tilt(const Matrix3& dcm, const Vector3& tilt);
template <typename S = Matrix, IfTensorOfDim<S, 2>* = nullptr>
Vector4 dcm_to_quat(const S& dcm) {
auto d0 = dcm(0, 0);
auto d1 = dcm(1, 1);
auto d2 = dcm(2, 2);
// Book just says max, but alternative formulation uses squares, so probably max(abs)
auto pa = std::fabs(1 + d0 + d1 + d2);
auto pb = std::fabs(1 + d0 - d1 - d2);
auto pc = std::fabs(1 - d0 + d1 - d2);
auto pd = std::fabs(1 - d0 - d1 + d2);
Scalar q0, q1, q2, q3;
if (pa >= pb && pa >= pc && pa >= pd) {
q0 = 0.5 * sqrt(pa);
auto q0t4 = 4 * q0;
q1 = (dcm(2, 1) - dcm(1, 2)) / (q0t4);
q2 = (dcm(0, 2) - dcm(2, 0)) / (q0t4);
q3 = (dcm(1, 0) - dcm(0, 1)) / (q0t4);
} else if (pb >= pa && pb >= pc && pb >= pd) {
q1 = 0.5 * sqrt(pb);
auto q1t4 = 4 * q1;
q0 = (dcm(2, 1) - dcm(1, 2)) / (q1t4);
q2 = (dcm(1, 0) + dcm(0, 1)) / (q1t4);
q3 = (dcm(0, 2) + dcm(2, 0)) / (q1t4);
} else if (pc >= pa && pc >= pb && pc >= pd) {
q2 = 0.5 * sqrt(pc);
auto q2t4 = 4 * q2;
q0 = (dcm(0, 2) - dcm(2, 0)) / (q2t4);
q1 = (dcm(1, 0) + dcm(0, 1)) / (q2t4);
q3 = (dcm(2, 1) + dcm(1, 2)) / (q2t4);
} else {
q3 = 0.5 * sqrt(pd);
auto q3t4 = 4 * q3;
q0 = (dcm(1, 0) - dcm(0, 1)) / (q3t4);
q1 = (dcm(0, 2) + dcm(2, 0)) / (q3t4);
q2 = (dcm(2, 1) + dcm(1, 2)) / (q3t4);
}
// use of sign bit is because -0.0 and 0.0 are technically both <= 0 and neither are < 0.
if (std::signbit(q0)) {
return {-q0, -q1, -q2, -q3};
} else {
return {q0, q1, q2, q3};
}
}
template <typename B, IfTensorOfDim<B, 3>* = nullptr>
Matrix dcm_to_quat(const B& dcm) {
const size_t N = dcm.shape()[0];
Matrix quats = empty(N, 4);
Scalar d0, d1, d2, pa, pb, pc, pd, q0, q1, q2, q3, q0t4, q1t4, q2t4, q3t4;
for (size_t i = 0; i < N; i++) {
d0 = dcm(i, 0, 0);
d1 = dcm(i, 1, 1);
d2 = dcm(i, 2, 2);
// Book just says max, but alternative formulation uses squares, so probably max(abs)
pa = std::fabs(1 + d0 + d1 + d2);
pb = std::fabs(1 + d0 - d1 - d2);
pc = std::fabs(1 - d0 + d1 - d2);
pd = std::fabs(1 - d0 - d1 + d2);
q0 = 0.0;
q1 = 0.0;
q2 = 0.0;
q3 = 0.0;
if (pa >= pb && pa >= pc && pa >= pd) {
q0 = 0.5 * sqrt(pa);
q0t4 = 4 * q0;
q1 = (dcm(i, 2, 1) - dcm(i, 1, 2)) / (q0t4);
q2 = (dcm(i, 0, 2) - dcm(i, 2, 0)) / (q0t4);
q3 = (dcm(i, 1, 0) - dcm(i, 0, 1)) / (q0t4);
} else if (pb >= pa && pb >= pc && pb >= pd) {
q1 = 0.5 * sqrt(pb);
q1t4 = 4 * q1;
q0 = (dcm(i, 2, 1) - dcm(i, 1, 2)) / (q1t4);
q2 = (dcm(i, 1, 0) + dcm(i, 0, 1)) / (q1t4);
q3 = (dcm(i, 0, 2) + dcm(i, 2, 0)) / (q1t4);
} else if (pc >= pa && pc >= pb && pc >= pd) {
q2 = 0.5 * sqrt(pc);
q2t4 = 4 * q2;
q0 = (dcm(i, 0, 2) - dcm(i, 2, 0)) / (q2t4);
q1 = (dcm(i, 1, 0) + dcm(i, 0, 1)) / (q2t4);
q3 = (dcm(i, 2, 1) + dcm(i, 1, 2)) / (q2t4);
} else {
q3 = 0.5 * sqrt(pd);
q3t4 = 4 * q3;
q0 = (dcm(i, 1, 0) - dcm(i, 0, 1)) / (q3t4);
q1 = (dcm(i, 0, 2) + dcm(i, 2, 0)) / (q3t4);
q2 = (dcm(i, 2, 1) + dcm(i, 1, 2)) / (q3t4);
}
// use of sign bit is because -0.0 and 0.0 are technically both <= 0.
if (std::signbit(q0)) {
quats(i, 0) = -q0;
quats(i, 1) = -q1;
quats(i, 2) = -q2;
quats(i, 3) = -q3;
} else {
quats(i, 0) = q0;
quats(i, 1) = q1;
quats(i, 2) = q2;
quats(i, 3) = q3;
}
}
return quats;
}
template <typename S = Matrix, IfTensorOfDim<S, 2>* = nullptr>
Vector3 dcm_to_rpy(const S& dcm) {
auto asin_arg = std::min(1.0, std::max(dcm(2, 0), -1.0));
auto r = atan2(dcm(2, 1), dcm(2, 2));
auto p = -asin(asin_arg);
auto y = atan2(dcm(1, 0), dcm(0, 0));
if (asin_arg <= -1 + 1e-12) {
auto y_min_r = atan2(dcm(1, 2) - dcm(0, 1), dcm(0, 2) + dcm(1, 1));
y = y_min_r + r;
}
if (asin_arg >= 1 - 1e-12) {
auto y_pls_r = atan2(dcm(1, 2) + dcm(0, 1), dcm(0, 2) - dcm(1, 1)) + PI;
y = remainder((y_pls_r - r), 2.0 * PI);
}
return {r, p, y};
}
template <typename B, IfTensorOfDim<B, 3>* = nullptr>
Matrix dcm_to_rpy(const B& dcm) {
const size_t N = dcm.shape()[0];
Matrix rpys = empty(N, 3);
Scalar asin_arg;
for (size_t i = 0; i < N; i++) {
asin_arg = std::min(1.0, std::max(dcm(i, 2, 0), -1.0));
rpys(i, 0) = atan2(dcm(i, 2, 1), dcm(i, 2, 2));
rpys(i, 1) = -asin(asin_arg);
rpys(i, 2) = atan2(dcm(i, 1, 0), dcm(i, 0, 0));
if (asin_arg <= -1 + 1e-12) {
auto y_min_r = atan2(dcm(i, 1, 2) - dcm(i, 0, 1), dcm(i, 0, 2) + dcm(i, 1, 1));
rpys(i, 2) = y_min_r + rpys(i, 0);
}
if (asin_arg >= 1 - 1e-12) {
auto y_pls_r = atan2(dcm(i, 1, 2) + dcm(i, 0, 1), dcm(i, 0, 2) - dcm(i, 1, 1)) + PI;
rpys(i, 2) = remainder((y_pls_r - rpys(i, 0)), 2.0 * PI);
}
}
return rpys;
}
template <typename S1, typename S2, typename S3, IfAllTensorsOfDim<0, S1, S2, S3>* = nullptr>
double delta_lat_to_north(const S1& delta_lat, const S2& approx_lat, const S3& altitude) {
return (meridian_radius(approx_lat) + altitude) * delta_lat;
}
template <typename B1 = Vector,
typename B2 = Vector,
typename B3 = Vector,
IfTensorsMaxDim<1, B1, B2, B3>* = nullptr>
Vector delta_lat_to_north(const B1& delta_lat, const B2& approx_lat, const B3& altitude) {
return (meridian_radius(approx_lat) + altitude) * delta_lat;
}
// default altitude of 0.0
#ifndef NEED_DOXYGEN_EXHALE_WORKAROUND
template <typename A = Vector, typename B = Vector>
auto delta_lat_to_north(const A& delta_lat, const B& approx_lat) {
return delta_lat_to_north(delta_lat, approx_lat, 0.0);
}
#endif
template <typename S1, typename S2, typename S3, IfAllTensorsOfDim<0, S1, S2, S3>* = nullptr>
double delta_lon_to_east(const S1& delta_lon, const S2& approx_lat, const S3& altitude) {
return (transverse_radius(approx_lat) + altitude) * delta_lon * cos(approx_lat);
}
template <typename B1 = Vector,
typename B2 = Vector,
typename B3 = Vector,
IfTensorsMaxDim<1, B1, B2, B3>* = nullptr>
Vector delta_lon_to_east(const B1& delta_lon, const B2& approx_lat, const B3& altitude) {
return (transverse_radius(approx_lat) + altitude) * delta_lon * cos(approx_lat);
}
// default altitude of 0.0
#ifndef NEED_DOXYGEN_EXHALE_WORKAROUND
template <typename A = Vector, typename B = Vector>
auto delta_lon_to_east(const A& delta_lon, const B& approx_lat) {
return delta_lon_to_east(delta_lon, approx_lat, 0.0);
}
#endif
std::pair<Matrix, Matrix> discretize_first_order(const Matrix& f, const Matrix& q, double dt);
std::pair<Matrix, Matrix> discretize_second_order(const Matrix& f, const Matrix& q, double dt);
std::pair<Matrix, Matrix> discretize_van_loan(const Matrix& f, const Matrix& q, double dt);
template <typename S1, typename S2, typename S3, IfAllTensorsOfDim<0, S1, S2, S3>* = nullptr>
double east_to_delta_lon(const S1& east_distance, const S2& approx_lat, const S3& altitude) {
return east_distance / ((transverse_radius(approx_lat) + altitude) * cos(approx_lat));
}
template <typename B1 = Vector,
typename B2 = Vector,
typename B3 = Vector,
IfTensorsMaxDim<1, B1, B2, B3>* = nullptr>
Vector east_to_delta_lon(const B1& east_distance, const B2& approx_lat, const B3& altitude) {
return east_distance / ((transverse_radius(approx_lat) + altitude) * cos(approx_lat));
}
// default altitude of 0.0
#ifndef NEED_DOXYGEN_EXHALE_WORKAROUND
template <typename A = Vector, typename B = Vector>
auto east_to_delta_lon(const A& east_distance, const B& approx_lat) {
return east_to_delta_lon(east_distance, approx_lat, 0.0);
}
#endif
Matrix3 ecef_to_cen(const Vector3& p_e);
template <typename S = Vector3, IfTensorOfDim<S, 1>* = nullptr>
Vector3 ecef_to_llh(const S& p_e) {
// WGS-84 Constants
auto a = SEMI_MAJOR_RADIUS; // Semi-major radius (m)
auto e2 = ECCENTRICITY_SQUARED; // Eccentricity squared (.)
double pm0 = sqrt(pow(p_e[0], 2) + pow(p_e[1], 2));
double pm1 = p_e[2];
double phi0 = atan2(pm1, pm0);
double h0 = 0;
double dp0 = a;
double dp1 = a;
int count = 0;
const int max_iterations = 5;
while ((std::abs(dp0) > 7e-6 || std::abs(dp1) > 1e-6) && count <= max_iterations) {
double slat = sin(phi0);
double clat = cos(phi0);
double s2lat = slat * slat;
double Nden = 1 - e2 * s2lat;
double N = a / sqrt(Nden);
// Calculate residual by subtracting initial position in meridianal plane (meters)
dp0 = pm0 - (N + h0) * clat;
dp1 = pm1 - (N * (1 - e2) + h0) * slat;
// Calculate inverse Jacobian (transformation from residual to lat and alt)
double k1 = 1 - e2 * s2lat;
double k2 = sqrt(k1);
double A11 = slat * (e2 * a * clat * clat / k1 / k2 - a / k2 - h0);
double A12 = clat;
double A21 = clat * (a * (1 - e2) / k2 + h0 + a * e2 * (1 - e2) * s2lat / k1 / k2);
double A22 = slat;
double Adet = A11 * A22 - A21 * A12;
double dHa = (A22 * dp0 - A12 * dp1) / Adet;
double dHb = (-A21 * dp0 + A11 * dp1) / Adet;
phi0 += dHa;
h0 += dHb;
++count;
}
double lam = atan2(p_e[1], p_e[0]);
return {phi0, lam, h0};
}
template <typename B = Matrix, IfTensorOfDim<B, 2>* = nullptr>
Matrix ecef_to_llh(const B& ecef) {
auto out = empty(ecef.shape()[0], 3);
const auto x = xt::view(ecef, xt::all(), 0);
const auto y = xt::view(ecef, xt::all(), 1);
const auto z = xt::view(ecef, xt::all(), 2);
const double a = SEMI_MAJOR_RADIUS;
const double e2 = ECCENTRICITY_SQUARED;
const double b = a * std::sqrt(1 - e2);
const double E2 = a * a - b * b;
// const double E = std::sqrt(E2);
const auto r2 = x * x + y * y + z * z;
const auto u = xt::sqrt(.5 * (r2 - E2) + .5 * xt::sqrt((r2 - E2) * (r2 - E2) + 4 * E2 * z * z));
const auto p = xt::sqrt(x * x + y * y);
const auto huE = xt::sqrt(u * u + E2);
const auto beta0 = xt::atan2(huE * z, u * p);
// first order correction
const auto sin_beta = xt::sin(beta0);
const auto cos_beta = xt::cos(beta0);
const auto eps = ((b * u - a * huE + E2) * sin_beta) / (a * huE / cos_beta - E2 * cos_beta);
const auto beta = beta0 + eps;
const auto lat = xt::atan((a / b) * xt::tan(beta));
const auto lon = xt::atan2(y, x);
const auto inside = x * x / (a * a) + y * y / (a * a) + z * z / (b * b) < 1;
const auto alt =
xt::sqrt(xt::square(z - b * xt::sin(beta)) + xt::square(p - a * xt::cos(beta)));
const auto alt_signed = xt::where(inside, -alt, alt);
xt::view(out, xt::all(), 0) = lat;
xt::view(out, xt::all(), 1) = lon;
xt::view(out, xt::all(), 2) = alt_signed;
return out;
}
Vector pressure_to_altitude(const Vector& pressure,
double ref_alt = 0,
double ref_pressure = 101325,
double ref_temp = 288.15);
Vector3 ecef_to_local_level(const Vector3& P0e, const Vector3& p_e);
Matrix3 llh_to_cen(const Vector3& p_wgs);
template <typename S = Vector3, IfTensorOfDim<S, 1>* = nullptr>
Vector3 llh_to_ecef(const S& p_wgs) {
// WGS-84 Constants
auto a = SEMI_MAJOR_RADIUS; // Semi-major radius (m)
auto e2 = ECCENTRICITY_SQUARED; // Eccentricity squared (.)
auto lam = p_wgs[1];
auto phi = p_wgs[0];
auto h = p_wgs[2];
auto cosphi = cos(phi);
auto sinphi = sin(phi);
auto N = a / sqrt(1 - e2 * pow(sinphi, 2));
return {(N + h) * cosphi * cos(lam), (N + h) * cosphi * sin(lam), (N * (1 - e2) + h) * sinphi};
}
template <typename B = Matrix, IfTensorOfDim<B, 2>* = nullptr>
Matrix llh_to_ecef(const B& llh) {
Matrix out = empty(llh.shape()[0], 3);
const auto lat = xt::view(llh, xt::all(), 0);
const auto lon = xt::view(llh, xt::all(), 1);
const auto alt = xt::view(llh, xt::all(), 2);
const auto sin_lat = xt::sin(lat);
const auto cos_lat = xt::cos(lat);
const auto radius =
SEMI_MAJOR_RADIUS / xt::sqrt(1.0 - ECCENTRICITY_SQUARED * xt::square(sin_lat));
xt::view(out, xt::all(), 0) = (radius + alt) * cos_lat * xt::cos(lon);
xt::view(out, xt::all(), 1) = (radius + alt) * cos_lat * xt::sin(lon);
xt::view(out, xt::all(), 2) = (radius * (1.0 - ECCENTRICITY_SQUARED) + alt) * sin_lat;
return out;
}
Vector3 local_level_to_ecef(const Vector3& P0e, const Vector3& Pn);
template <typename S1, typename S2, typename S3, IfAllTensorsOfDim<0, S1, S2, S3>* = nullptr>
double north_to_delta_lat(const S1& north_distance, const S2& approx_lat, const S3& altitude) {
return north_distance / (meridian_radius(approx_lat) + altitude);
}
template <typename B1 = Vector,
typename B2 = Vector,
typename B3 = Vector,
IfTensorsMaxDim<1, B1, B2, B3>* = nullptr>
Vector north_to_delta_lat(const B1& north_distance, const B2& approx_lat, const B3& altitude) {
return north_distance / (meridian_radius(approx_lat) + altitude);
}
// default altitude of 0.0
#ifndef NEED_DOXYGEN_EXHALE_WORKAROUND
template <typename A = Vector, typename B = Vector>
auto north_to_delta_lat(const A& north_distance, const B& approx_lat) {
return north_to_delta_lat(north_distance, approx_lat, 0.0);
}
#endif
template <typename S = Vector, IfTensorOfDim<S, 1>* = nullptr>
Matrix3 quat_to_dcm(const S& quat) {
auto q0 = quat(0);
auto q1 = quat(1);
auto q2 = quat(2);
auto q3 = quat(3);
auto a2 = pow(q0, 2);
auto b2 = pow(q1, 2);
auto c2 = pow(q2, 2);
auto d2 = pow(q3, 2);
auto ab = q0 * q1;
auto ac = q0 * q2;
auto ad = q0 * q3;
auto bc = q1 * q2;
auto bd = q1 * q3;
auto cd = q2 * q3;
return {{a2 + b2 - c2 - d2, 2 * (bc - ad), 2 * (bd + ac)},
{2 * (bc + ad), a2 - b2 + c2 - d2, 2 * (cd - ab)},
{2 * (bd - ac), 2 * (cd + ab), a2 - b2 - c2 + d2}};
}
template <typename B = Matrix, IfTensorOfDim<B, 2>* = nullptr>
Tensor<3> quat_to_dcm(const B& quat) {
const size_t N = quat.shape()[0];
Tensor<3> dcms = empty(N, 3, 3);
Scalar q0, q1, q2, q3, a2, b2, c2, d2, ab, ac, ad, bc, bd, cd;
for (size_t i = 0; i < N; i++) {
q0 = quat(i, 0);
q1 = quat(i, 1);
q2 = quat(i, 2);
q3 = quat(i, 3);
a2 = pow(q0, 2);
b2 = pow(q1, 2);
c2 = pow(q2, 2);
d2 = pow(q3, 2);
ab = q0 * q1;
ac = q0 * q2;
ad = q0 * q3;
bc = q1 * q2;
bd = q1 * q3;
cd = q2 * q3;
dcms(i, 0, 0) = a2 + b2 - c2 - d2;
dcms(i, 0, 1) = 2 * (bc - ad);
dcms(i, 0, 2) = 2 * (bd + ac);
dcms(i, 1, 0) = 2 * (bc + ad);
dcms(i, 1, 1) = a2 - b2 + c2 - d2;
dcms(i, 1, 2) = 2 * (cd - ab);
dcms(i, 2, 0) = 2 * (bd - ac);
dcms(i, 2, 1) = 2 * (cd + ab);
dcms(i, 2, 2) = a2 - b2 - c2 + d2;
}
return dcms;
}
template <typename S = Vector, IfTensorOfDim<S, 1>* = nullptr>
Vector3 quat_to_rpy(const S& quat) {
auto q0 = quat[0];
auto q1 = quat[1];
auto q2 = quat[2];
auto q3 = quat[3];
auto roll = atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2)),
pitch = asin(std::min(std::max(2 * (q0 * q2 - q1 * q3), -1.0), 1.0)),
yaw = atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
return {roll, pitch, yaw};
}
template <typename B = Matrix, IfTensorOfDim<B, 2>* = nullptr>
Matrix quat_to_rpy(const B& quat) {
const size_t N = quat.shape()[0];
// allocate return Matrix
Matrix rpys = empty(N, 3);
Scalar q0, q1, q2, q3;
for (size_t i = 0; i < N; i++) {
q0 = quat(i, 0);
q1 = quat(i, 1);
q2 = quat(i, 2);
q3 = quat(i, 3);
// roll, pitch, and yaw
rpys(i, 0) = atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));
rpys(i, 1) = asin(std::min(std::max(2 * (q0 * q2 - q1 * q3), -1.0), 1.0));
rpys(i, 2) = atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
}
return rpys;
}
template <typename S = Vector, IfTensorOfDim<S, 1>* = nullptr>
Matrix3 rpy_to_dcm(const S& rpy) {
Scalar cph = cos(rpy[0]);
Scalar sph = sin(rpy[0]);
Scalar cth = cos(rpy[1]);
Scalar sth = sin(rpy[1]);
Scalar cps = cos(rpy[2]);
Scalar sps = sin(rpy[2]);
return Matrix{{cps * cth, -sps * cph + cps * sth * sph, sps * sph + cps * sth * cph},
{sps * cth, cps * cph + sps * sth * sph, -cps * sph + sps * sth * cph},
{-sth, cth * sph, cth * cph}};
}
template <typename B = Matrix, IfTensorOfDim<B, 2>* = nullptr>
Tensor<3> rpy_to_dcm(const B& rpy) {
const size_t N = rpy.shape()[0];
Tensor<3> dcms = empty(N, 3, 3);
Scalar cph, sph, cth, sth, cps, sps;
for (size_t i = 0; i < N; i++) {
// Negate angles as we are performing
// left-handed coordinate rotations
cph = cos(rpy(i, 0));
sph = sin(rpy(i, 0));
cth = cos(rpy(i, 1));
sth = sin(rpy(i, 1));
cps = cos(rpy(i, 2));
sps = sin(rpy(i, 2));
dcms(i, 0, 0) = cps * cth;
dcms(i, 0, 1) = -sps * cph + cps * sth * sph;
dcms(i, 0, 2) = sps * sph + cps * sth * cph;
dcms(i, 1, 0) = sps * cth;
dcms(i, 1, 1) = cps * cph + sps * sth * sph;
dcms(i, 1, 2) = -cps * sph + sps * sth * cph;
dcms(i, 2, 0) = -sth;
dcms(i, 2, 1) = cth * sph;
dcms(i, 2, 2) = cth * cph;
}
return dcms;
}
template <typename S = Vector, IfTensorOfDim<S, 1>* = nullptr>
Vector4 rpy_to_quat(const S& rpy) {
auto cr = cos(rpy[0] / 2), cp = cos(rpy[1] / 2), cy = cos(rpy[2] / 2), sr = sin(rpy[0] / 2),
sp = sin(rpy[1] / 2), sy = sin(rpy[2] / 2);
return {cr * cp * cy + sr * sp * sy,
sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy,
cr * cp * sy - sr * sp * cy};
}
template <typename B = Matrix, IfTensorOfDim<B, 2>* = nullptr>
Matrix rpy_to_quat(const B& rpy) {
const size_t N = rpy.shape()[0];
// allocate return Matrix
Matrix quats = empty(N, 4);
Scalar cr, cp, cy, sr, sp, sy;
for (size_t i = 0; i < N; i++) {
cr = cos(rpy(i, 0) / 2);
cp = cos(rpy(i, 1) / 2);
cy = cos(rpy(i, 2) / 2);
sr = sin(rpy(i, 0) / 2);
sp = sin(rpy(i, 1) / 2);
sy = sin(rpy(i, 2) / 2);
quats(i, 0) = cr * cp * cy + sr * sp * sy;
quats(i, 1) = sr * cp * cy - cr * sp * sy;
quats(i, 2) = cr * sp * cy + sr * cp * sy;
quats(i, 3) = cr * cp * sy - sr * sp * cy;
}
return quats;
}
Matrix3 wander_to_C_enu_to_n(double wander);
Matrix3 wander_to_C_ned_to_n(double wander);
Matrix3 wander_to_C_ned_to_l(double wander);
Matrix3 lat_lon_wander_to_C_n_to_e(double lat, double lon, double wander = 0.0);
Vector3 C_n_to_e_to_lat_lon_wander(const Matrix& C_n_to_e);
double C_n_to_e_to_wander(const Matrix3& C_n_to_e);
std::pair<Matrix3, double> ecef_wander_to_C_n_to_e_h(const Vector3& ecef_pos, double wander = 0.0);
Vector3 C_n_to_e_h_to_llh(const Matrix3& C_n_to_e, double h);
Vector3 C_n_to_e_h_to_ecef(const Matrix3& C_n_to_e, double h);
Matrix3 C_ecef_to_e();
template <typename S = Vector, IfTensorOfDim<S, 1>* = nullptr>
Matrix3 rot_vec_to_dcm(const S& phi) {
/*
// 'Normal' implementation. Long chains of xtensor related operations can cause large slowdowns,
// especially w/ ASAN testing, so actual implementation does math 'manually' to achieve speed.
double phi_mag = xt::norm_l2(phi)[0];
double phi_mag2 = phi_mag * phi_mag;
double phi_mag4 = phi_mag2 * phi_mag2;
double term1 = 1 - phi_mag2 / 6 + phi_mag4 / 120;
double term2 = 0.5 - phi_mag2 / 24 + phi_mag4 / 720;
auto phi_cross = skew(phi);
return eye(3) + term1 * phi_cross + term2 * dot(phi_cross, phi_cross);
*/
auto p0 = phi(0);
auto p1 = phi(1);
auto p2 = phi(2);
double phi_mag = sqrt(p0 * p0 + p1 * p1 + p2 * p2);
double phi_mag2 = phi_mag * phi_mag;
double phi_mag4 = phi_mag2 * phi_mag2;
double t1 = 1 - phi_mag2 / 6 + phi_mag4 / 120;
double t2 = 0.5 - phi_mag2 / 24 + phi_mag4 / 720;
return {{1.0 + t2 * (-p2 * p2 - p1 * p1), -t1 * p2 + t2 * p1 * p0, t1 * p1 + t2 * p0 * p2},
{t1 * p2 + t2 * p0 * p1, 1.0 + t2 * (-p2 * p2 - p0 * p0), -t1 * p0 + t2 * p1 * p2},
{-t1 * p1 + t2 * p0 * p2, t1 * p0 + t2 * p1 * p2, 1.0 + t2 * (-p1 * p1 - p0 * p0)}};
}
template <typename B = Matrix, IfTensorOfDim<B, 2>* = nullptr>
Tensor<3> rot_vec_to_dcm(const B& phi) {
const size_t N = phi.shape()[0];
Tensor<3> dcms = empty(N, 3, 3);
Scalar p0, p1, p2, phi_mag, phi_mag2, phi_mag4, t1, t2;
for (size_t i = 0; i < N; i++) {
p0 = phi(i, 0);
p1 = phi(i, 1);
p2 = phi(i, 2);
phi_mag = sqrt(p0 * p0 + p1 * p1 + p2 * p2);
phi_mag2 = phi_mag * phi_mag;
phi_mag4 = phi_mag2 * phi_mag2;
t1 = 1 - phi_mag2 / 6 + phi_mag4 / 120;
t2 = 0.5 - phi_mag2 / 24 + phi_mag4 / 720;
dcms(i, 0, 0) = 1.0 + t2 * (-p2 * p2 - p1 * p1);
dcms(i, 0, 1) = -t1 * p2 + t2 * p1 * p0;
dcms(i, 0, 2) = t1 * p1 + t2 * p0 * p2;
dcms(i, 1, 0) = t1 * p2 + t2 * p0 * p1;
dcms(i, 1, 1) = 1.0 + t2 * (-p2 * p2 - p0 * p0);
dcms(i, 1, 2) = -t1 * p0 + t2 * p1 * p2;
dcms(i, 2, 0) = -t1 * p1 + t2 * p0 * p2;
dcms(i, 2, 1) = t1 * p0 + t2 * p1 * p2;
dcms(i, 2, 2) = 1.0 + t2 * (-p1 * p1 - p0 * p0);
}
return dcms;
}
Matrix dcm_to_rot_vec(const Tensor<3, double>& dcms);
std::pair<bool, double> geoid_minus_ellipsoid(double latitude,
double longitude,
const std::string& path = "WW15MGH.GRD");
std::pair<bool, double> hae_to_msl(double hae,
double latitude,
double longitude,
const std::string& path = "WW15MGH.GRD");
Matrix llh_to_ned(const Matrix& llh, const Vector3& llh0);
Matrix ned_to_llh(const Matrix& ned, const Vector3& llh0);
Matrix ned_sigma_to_llh_sigma(const Matrix& ned_sigma, const Vector3& llh0);
Matrix llh_sigma_to_ned_sigma(const Matrix& llh_sigma, const Vector3& llh0);
std::pair<bool, double> msl_to_hae(double msl,
double latitude,
double longitude,
const std::string& path = "WW15MGH.GRD");
} // namespace navutils
} // namespace navtk