mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-11-16 23:04:57 +00:00
776 lines
26 KiB
C++
776 lines
26 KiB
C++
/*!
|
|
* \file geofunctions.cc
|
|
* \brief A set of coordinate transformations functions and helpers,
|
|
* some of them migrated from MATLAB, for geographic information systems.
|
|
* \author Javier Arribas, 2018. jarribas(at)cttc.es
|
|
*
|
|
* -------------------------------------------------------------------------
|
|
*
|
|
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
|
|
*
|
|
* GNSS-SDR is a software defined Global Navigation
|
|
* Satellite Systems receiver
|
|
*
|
|
* This file is part of GNSS-SDR.
|
|
*
|
|
* GNSS-SDR is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* GNSS-SDR is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
|
|
*
|
|
* -------------------------------------------------------------------------
|
|
*/
|
|
|
|
#include "geofunctions.h"
|
|
|
|
const double STRP_PI = 3.1415926535898; // Pi as defined in IS-GPS-200E
|
|
|
|
arma::mat Skew_symmetric(const arma::vec &a)
|
|
{
|
|
arma::mat A = arma::zeros(3, 3);
|
|
|
|
A << 0.0 << -a(2) << a(1) << arma::endr
|
|
<< a(2) << 0.0 << -a(0) << arma::endr
|
|
<< -a(1) << a(0) << 0 << arma::endr;
|
|
|
|
// {{0, -a(2), a(1)},
|
|
// {a(2), 0, -a(0)},
|
|
// {-a(1), a(0), 0}};
|
|
return A;
|
|
}
|
|
|
|
|
|
double WGS84_g0(double Lat_rad)
|
|
{
|
|
const double k = 0.001931853; // normal gravity constant
|
|
const double e2 = 0.00669438002290; // the square of the first numerical eccentricity
|
|
const double nge = 9.7803253359; // normal gravity value on the equator (m/sec^2)
|
|
double b = sin(Lat_rad); // Lat in degrees
|
|
b = b * b;
|
|
double g0 = nge * (1 + k * b) / (sqrt(1 - e2 * b));
|
|
return g0;
|
|
}
|
|
|
|
|
|
double WGS84_geocentric_radius(double Lat_geodetic_rad)
|
|
{
|
|
// WGS84 earth model Geocentric radius (Eq. 2.88)
|
|
const double WGS84_A = 6378137.0; // Semi-major axis of the Earth, a [m]
|
|
const double WGS84_IF = 298.257223563; // Inverse flattening of the Earth
|
|
const double WGS84_F = (1.0 / WGS84_IF); // The flattening of the Earth
|
|
// double WGS84_B=(WGS84_A*(1-WGS84_F)); // Semi-minor axis of the Earth [m]
|
|
double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F)); // Eccentricity of the Earth
|
|
|
|
// transverse radius of curvature
|
|
double R_E = WGS84_A / sqrt(1 - WGS84_E * WGS84_E * sin(Lat_geodetic_rad) * sin(Lat_geodetic_rad)); // (Eq. 2.66)
|
|
|
|
// geocentric radius at the Earth surface
|
|
double r_eS = R_E * sqrt(cos(Lat_geodetic_rad) * cos(Lat_geodetic_rad) +
|
|
(1 - WGS84_E * WGS84_E) * (1 - WGS84_E * WGS84_E) * sin(Lat_geodetic_rad) * sin(Lat_geodetic_rad)); // (Eq. 2.88)
|
|
return r_eS;
|
|
}
|
|
|
|
|
|
int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx)
|
|
{
|
|
double lambda;
|
|
double phi;
|
|
double h;
|
|
const double dtr = STRP_PI / 180.0;
|
|
const double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84
|
|
const double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84
|
|
|
|
// Transform x into geodetic coordinates
|
|
togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
|
|
|
|
double cl = cos(lambda * dtr);
|
|
double sl = sin(lambda * dtr);
|
|
double cb = cos(phi * dtr);
|
|
double sb = sin(phi * dtr);
|
|
|
|
arma::mat F = {{-sl, -sb * cl, cb * cl},
|
|
{cl, -sb * sl, cb * sl},
|
|
{0.0, cb, sb}};
|
|
|
|
arma::vec local_vector;
|
|
|
|
local_vector = arma::htrans(F) * dx;
|
|
|
|
double E = local_vector(0);
|
|
double N = local_vector(1);
|
|
double U = local_vector(2);
|
|
|
|
double hor_dis;
|
|
hor_dis = sqrt(E * E + N * N);
|
|
|
|
if (hor_dis < 1.0E-20)
|
|
{
|
|
*Az = 0.0;
|
|
*El = 90.0;
|
|
}
|
|
else
|
|
{
|
|
*Az = atan2(E, N) / dtr;
|
|
*El = atan2(U, hor_dis) / dtr;
|
|
}
|
|
|
|
if (*Az < 0)
|
|
{
|
|
*Az = *Az + 360.0;
|
|
}
|
|
|
|
*D = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2));
|
|
return 0;
|
|
}
|
|
|
|
|
|
int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z)
|
|
{
|
|
*h = 0.0;
|
|
const double tolsq = 1.e-10; // tolerance to accept convergence
|
|
const int maxit = 10; // max number of iterations
|
|
const double rtd = 180.0 / STRP_PI;
|
|
|
|
// compute square of eccentricity
|
|
double esq;
|
|
if (finv < 1.0E-20)
|
|
{
|
|
esq = 0.0;
|
|
}
|
|
else
|
|
{
|
|
esq = (2.0 - 1.0 / finv) / finv;
|
|
}
|
|
|
|
// first guess
|
|
double P = sqrt(X * X + Y * Y); // P is distance from spin axis
|
|
|
|
// direct calculation of longitude
|
|
if (P > 1.0E-20)
|
|
{
|
|
*dlambda = atan2(Y, X) * rtd;
|
|
}
|
|
else
|
|
{
|
|
*dlambda = 0.0;
|
|
}
|
|
|
|
// correct longitude bound
|
|
if (*dlambda < 0)
|
|
{
|
|
*dlambda = *dlambda + 360.0;
|
|
}
|
|
|
|
double r = sqrt(P * P + Z * Z); // r is distance from origin (0,0,0)
|
|
|
|
double sinphi;
|
|
if (r > 1.0E-20)
|
|
{
|
|
sinphi = Z / r;
|
|
}
|
|
else
|
|
{
|
|
sinphi = 0.0;
|
|
}
|
|
*dphi = asin(sinphi);
|
|
|
|
// initial value of height = distance from origin minus
|
|
// approximate distance from origin to surface of ellipsoid
|
|
if (r < 1.0E-20)
|
|
{
|
|
*h = 0.0;
|
|
return 1;
|
|
}
|
|
|
|
*h = r - a * (1 - sinphi * sinphi / finv);
|
|
|
|
// iterate
|
|
double cosphi;
|
|
double N_phi;
|
|
double dP;
|
|
double dZ;
|
|
double oneesq = 1.0 - esq;
|
|
|
|
for (int i = 0; i < maxit; i++)
|
|
{
|
|
sinphi = sin(*dphi);
|
|
cosphi = cos(*dphi);
|
|
|
|
// compute radius of curvature in prime vertical direction
|
|
N_phi = a / sqrt(1.0 - esq * sinphi * sinphi);
|
|
|
|
// compute residuals in P and Z
|
|
dP = P - (N_phi + (*h)) * cosphi;
|
|
dZ = Z - (N_phi * oneesq + (*h)) * sinphi;
|
|
|
|
// update height and latitude
|
|
*h = *h + (sinphi * dZ + cosphi * dP);
|
|
*dphi = *dphi + (cosphi * dZ - sinphi * dP) / (N_phi + (*h));
|
|
|
|
// test for convergence
|
|
if ((dP * dP + dZ * dZ) < tolsq)
|
|
{
|
|
break;
|
|
}
|
|
if (i == (maxit - 1))
|
|
{
|
|
// LOG(WARNING) << "The computation of geodetic coordinates did not converge";
|
|
}
|
|
}
|
|
*dphi = (*dphi) * rtd;
|
|
return 0;
|
|
}
|
|
|
|
|
|
arma::mat Gravity_ECEF(const arma::vec &r_eb_e)
|
|
{
|
|
// Parameters
|
|
const double R_0 = 6378137.0; // WGS84 Equatorial radius in meters
|
|
const double mu = 3.986004418E14; // WGS84 Earth gravitational constant (m^3 s^-2)
|
|
const double J_2 = 1.082627E-3; // WGS84 Earth's second gravitational constant
|
|
const double omega_ie = 7.292115E-5; // Earth rotation rate (rad/s)
|
|
// Calculate distance from center of the Earth
|
|
double mag_r = sqrt(arma::as_scalar(r_eb_e.t() * r_eb_e));
|
|
// If the input position is 0,0,0, produce a dummy output
|
|
arma::vec g = arma::zeros(3, 1);
|
|
if (mag_r != 0)
|
|
{
|
|
// Calculate gravitational acceleration using (2.142)
|
|
double z_scale = 5 * pow((r_eb_e(2) / mag_r), 2);
|
|
arma::vec tmp_vec = {(1 - z_scale) * r_eb_e(0),
|
|
(1 - z_scale) * r_eb_e(1),
|
|
(3 - z_scale) * r_eb_e(2)};
|
|
arma::vec gamma_ = (-mu / pow(mag_r, 3)) * (r_eb_e + 1.5 * J_2 * pow(R_0 / mag_r, 2) * tmp_vec);
|
|
|
|
// Add centripetal acceleration using (2.133)
|
|
g(0) = gamma_(0) + pow(omega_ie, 2) * r_eb_e(0);
|
|
g(1) = gamma_(1) + pow(omega_ie, 2) * r_eb_e(1);
|
|
g(2) = gamma_(2);
|
|
}
|
|
return g;
|
|
}
|
|
|
|
|
|
arma::vec LLH_to_deg(const arma::vec &LLH)
|
|
{
|
|
const double rtd = 180.0 / STRP_PI;
|
|
arma::vec deg = arma::zeros(3, 1);
|
|
deg(0) = LLH(0) * rtd;
|
|
deg(1) = LLH(1) * rtd;
|
|
deg(2) = LLH(2);
|
|
return deg;
|
|
}
|
|
|
|
|
|
double degtorad(double angleInDegrees)
|
|
{
|
|
double angleInRadians = (STRP_PI / 180.0) * angleInDegrees;
|
|
return angleInRadians;
|
|
}
|
|
|
|
|
|
double radtodeg(double angleInRadians)
|
|
{
|
|
double angleInDegrees = (180.0 / STRP_PI) * angleInRadians;
|
|
return angleInDegrees;
|
|
}
|
|
|
|
|
|
double mstoknotsh(double MetersPerSeconds)
|
|
{
|
|
double knots = mstokph(MetersPerSeconds) * 0.539957;
|
|
return knots;
|
|
}
|
|
|
|
|
|
double mstokph(double MetersPerSeconds)
|
|
{
|
|
double kph = 3600.0 * MetersPerSeconds / 1e3;
|
|
return kph;
|
|
}
|
|
|
|
|
|
arma::vec CTM_to_Euler(const arma::mat &C)
|
|
{
|
|
// Calculate Euler angles using (2.23)
|
|
arma::mat CTM = C;
|
|
arma::vec eul = arma::zeros(3, 1);
|
|
eul(0) = atan2(CTM(1, 2), CTM(2, 2)); // roll
|
|
if (CTM(0, 2) < -1.0) CTM(0, 2) = -1.0;
|
|
if (CTM(0, 2) > 1.0) CTM(0, 2) = 1.0;
|
|
eul(1) = -asin(CTM(0, 2)); // pitch
|
|
eul(2) = atan2(CTM(0, 1), CTM(0, 0)); // yaw
|
|
return eul;
|
|
}
|
|
|
|
|
|
arma::mat Euler_to_CTM(const arma::vec &eul)
|
|
{
|
|
// Eq.2.15
|
|
// Euler angles to Attitude matrix is equivalent to rotate the body
|
|
// in the three axes:
|
|
// arma::mat Ax= {{1,0,0}, {0,cos(Att_phi),sin(Att_phi)} ,{0,-sin(Att_phi),cos(Att_phi)}};
|
|
// arma::mat Ay= {{cos(Att_theta), 0, -sin(Att_theta)}, {0,1,0} , {sin(Att_theta), 0, cos(Att_theta)}};
|
|
// arma::mat Az= {{cos(Att_psi), sin(Att_psi), 0}, {-sin(Att_psi), cos(Att_psi), 0},{0,0,1}};
|
|
// arma::mat C_b_n=Ax*Ay*Az; // Attitude expressed in the LOCAL FRAME (NED)
|
|
// C_b_n=C_b_n.t();
|
|
|
|
// Precalculate sines and cosines of the Euler angles
|
|
double sin_phi = sin(eul(0));
|
|
double cos_phi = cos(eul(0));
|
|
double sin_theta = sin(eul(1));
|
|
double cos_theta = cos(eul(1));
|
|
double sin_psi = sin(eul(2));
|
|
double cos_psi = cos(eul(2));
|
|
|
|
// Calculate coordinate transformation matrix using (2.22)
|
|
arma::mat C = {{cos_theta * cos_psi, cos_theta * sin_psi, -sin_theta},
|
|
{-cos_phi * sin_psi + sin_phi * sin_theta * cos_psi, cos_phi * cos_psi + sin_phi * sin_theta * sin_psi, sin_phi * cos_theta},
|
|
{sin_phi * sin_psi + cos_phi * sin_theta * cos_psi, -sin_phi * cos_psi + cos_phi * sin_theta * sin_psi, cos_phi * cos_theta}};
|
|
return C;
|
|
}
|
|
|
|
|
|
arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection)
|
|
{
|
|
const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0};
|
|
const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563};
|
|
|
|
double lambda = atan2(XYZ[1], XYZ[0]);
|
|
double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection]));
|
|
double c = a[elipsoid_selection] * sqrt(1.0 + ex2);
|
|
double phi = atan(XYZ[2] / ((sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection])));
|
|
|
|
double h = 0.1;
|
|
double oldh = 0.0;
|
|
double N;
|
|
int iterations = 0;
|
|
do
|
|
{
|
|
oldh = h;
|
|
N = c / sqrt(1.0 + ex2 * (cos(phi) * cos(phi)));
|
|
phi = atan(XYZ[2] / ((sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h)))));
|
|
h = sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) / cos(phi) - N;
|
|
iterations = iterations + 1;
|
|
if (iterations > 100)
|
|
{
|
|
// std::cout << "Failed to approximate h with desired precision. h-oldh= " << h - oldh;
|
|
break;
|
|
}
|
|
}
|
|
while (std::fabs(h - oldh) > 1.0e-12);
|
|
|
|
arma::vec LLH = {{phi, lambda, h}}; // radians
|
|
return LLH;
|
|
}
|
|
|
|
|
|
void ECEF_to_Geo(const arma::vec &r_eb_e, const arma::vec &v_eb_e, const arma::mat &C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n)
|
|
{
|
|
// Compute the Latitude of the ECEF position
|
|
LLH = cart2geo(r_eb_e, 4); // ECEF -> WGS84 geographical
|
|
|
|
// Calculate ECEF to Geographical coordinate transformation matrix using (2.150)
|
|
double cos_lat = cos(LLH(0));
|
|
double sin_lat = sin(LLH(0));
|
|
double cos_long = cos(LLH(1));
|
|
double sin_long = sin(LLH(1));
|
|
// C++11 and arma >= 5.2
|
|
// arma::mat C_e_n = {{-sin_lat * cos_long, -sin_lat * sin_long, cos_lat},
|
|
// {-sin_long, cos_long, 0},
|
|
// {-cos_lat * cos_long, -cos_lat * sin_long, -sin_lat}}; //ECEF to Geo
|
|
arma::mat C_e_n = arma::zeros(3, 3);
|
|
C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr
|
|
<< -sin_long << cos_long << 0 << arma::endr
|
|
<< -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr; // ECEF to Geo
|
|
// Transform velocity using (2.73)
|
|
v_eb_n = C_e_n * v_eb_e;
|
|
|
|
C_b_n = C_e_n * C_b_e; // Attitude conversion from ECEF to NED
|
|
}
|
|
|
|
|
|
void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat &C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e)
|
|
{
|
|
// Parameters
|
|
double R_0 = 6378137.0; // WGS84 Equatorial radius in meters
|
|
double e = 0.0818191908425; // WGS84 eccentricity
|
|
|
|
// Calculate transverse radius of curvature using (2.105)
|
|
double R_E = R_0 / sqrt(1.0 - (e * sin(LLH(0))) * (e * sin(LLH(0))));
|
|
|
|
// Convert position using (2.112)
|
|
double cos_lat = cos(LLH(0));
|
|
double sin_lat = sin(LLH(0));
|
|
double cos_long = cos(LLH(1));
|
|
double sin_long = sin(LLH(1));
|
|
r_eb_e = {(R_E + LLH(2)) * cos_lat * cos_long,
|
|
(R_E + LLH(2)) * cos_lat * sin_long,
|
|
((1 - e * e) * R_E + LLH(2)) * sin_lat};
|
|
|
|
// Calculate ECEF to Geo coordinate transformation matrix using (2.150)
|
|
// C++11 and arma>=5.2
|
|
// arma::mat C_e_n = {{-sin_lat * cos_long, -sin_lat * sin_long, cos_lat},
|
|
// {-sin_long, cos_long, 0},
|
|
// {-cos_lat * cos_long, -cos_lat * sin_long, -sin_lat}};
|
|
arma::mat C_e_n = arma::zeros(3, 3);
|
|
C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr
|
|
<< -sin_long << cos_long << 0 << arma::endr
|
|
<< -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr;
|
|
|
|
// Transform velocity using (2.73)
|
|
v_eb_e = C_e_n.t() * v_eb_n;
|
|
|
|
// Transform attitude using (2.15)
|
|
C_b_e = C_e_n.t() * C_b_n;
|
|
}
|
|
|
|
|
|
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e)
|
|
{
|
|
// Parameters
|
|
const double R_0 = 6378137.0; // WGS84 Equatorial radius in meters
|
|
const double e = 0.0818191908425; // WGS84 eccentricity
|
|
|
|
// Calculate transverse radius of curvature using (2.105)
|
|
double R_E = R_0 / sqrt(1 - pow(e * sin(L_b), 2));
|
|
|
|
// Convert position using (2.112)
|
|
double cos_lat = cos(L_b);
|
|
double sin_lat = sin(L_b);
|
|
double cos_long = cos(lambda_b);
|
|
double sin_long = sin(lambda_b);
|
|
r_eb_e = {(R_E + h_b) * cos_lat * cos_long,
|
|
(R_E + h_b) * cos_lat * sin_long,
|
|
((1 - pow(e, 2)) * R_E + h_b) * sin_lat};
|
|
|
|
// Calculate ECEF to Geo coordinate transformation matrix using (2.150)
|
|
arma::mat C_e_n = arma::zeros(3, 3);
|
|
C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr
|
|
<< -sin_long << cos_long << 0 << arma::endr
|
|
<< -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr;
|
|
|
|
// Transform velocity using (2.73)
|
|
v_eb_e = C_e_n.t() * v_eb_n;
|
|
}
|
|
|
|
|
|
double great_circle_distance(double lat1, double lon1, double lat2, double lon2)
|
|
{
|
|
// The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes.
|
|
// generally used geo measurement function
|
|
double R = 6378.137; // Radius of earth in KM
|
|
double dLat = lat2 * STRP_PI / 180.0 - lat1 * STRP_PI / 180.0;
|
|
double dLon = lon2 * STRP_PI / 180.0 - lon1 * STRP_PI / 180.0;
|
|
double a = sin(dLat / 2.0) * sin(dLat / 2.0) +
|
|
cos(lat1 * STRP_PI / 180.0) * cos(lat2 * STRP_PI / 180.0) *
|
|
sin(dLon / 2) * sin(dLon / 2.0);
|
|
double c = 2.0 * atan2(sqrt(a), sqrt(1.0 - a));
|
|
double d = R * c;
|
|
return d * 1000.0; // meters
|
|
}
|
|
|
|
|
|
void cart2utm(const arma::vec &r_eb_e, int zone, arma::vec &r_enu)
|
|
{
|
|
// Transformation of (X,Y,Z) to (E,N,U) in UTM, zone 'zone'
|
|
//
|
|
// Inputs:
|
|
// r_eb_e - Cartesian coordinates. Coordinates are referenced
|
|
// with respect to the International Terrestrial Reference
|
|
// Frame 1996 (ITRF96)
|
|
// zone - UTM zone of the given position
|
|
//
|
|
// Outputs:
|
|
// r_enu - UTM coordinates (Easting, Northing, Uping)
|
|
//
|
|
// Originally written in Matlab by Kai Borre, Nov. 1994
|
|
// Implemented in C++ by J.Arribas
|
|
//
|
|
// This implementation is based upon
|
|
// O. Andersson & K. Poder (1981) Koordinattransformationer
|
|
// ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren
|
|
// Vol. 30: 552--571 and Vol. 31: 76
|
|
//
|
|
// An excellent, general reference (KW) is
|
|
// R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der
|
|
// h\"oheren Geod\"asie und Kartographie.
|
|
// Erster Band, Springer Verlag
|
|
//
|
|
// Explanation of variables used:
|
|
// f flattening of ellipsoid
|
|
// a semi major axis in m
|
|
// m0 1 - scale at central meridian; for UTM 0.0004
|
|
// Q_n normalized meridian quadrant
|
|
// E0 Easting of central meridian
|
|
// L0 Longitude of central meridian
|
|
// bg constants for ellipsoidal geogr. to spherical geogr.
|
|
// gb constants for spherical geogr. to ellipsoidal geogr.
|
|
// gtu constants for ellipsoidal N, E to spherical N, E
|
|
// utg constants for spherical N, E to ellipoidal N, E
|
|
// tolutm tolerance for utm, 1.2E-10*meridian quadrant
|
|
// tolgeo tolerance for geographical, 0.00040 second of arc
|
|
//
|
|
// B, L refer to latitude and longitude. Southern latitude is negative
|
|
// International ellipsoid of 1924, valid for ED50
|
|
|
|
double a = 6378388.0;
|
|
double f = 1.0 / 297.0;
|
|
double ex2 = (2.0 - f) * f / ((1.0 - f) * (1.0 - f));
|
|
double c = a * sqrt(1.0 + ex2);
|
|
arma::vec vec = r_eb_e;
|
|
vec(2) = vec(2) - 4.5;
|
|
double alpha = 0.756e-6;
|
|
arma::mat R = {{1.0, -alpha, 0.0}, {alpha, 1.0, 0.0}, {0.0, 0.0, 1.0}};
|
|
arma::vec trans = {89.5, 93.8, 127.6};
|
|
double scale = 0.9999988;
|
|
arma::vec v = scale * R * vec + trans; // coordinate vector in ED50
|
|
double L = atan2(v(1), v(0));
|
|
double N1 = 6395000.0; // preliminary value
|
|
double B = atan2(v(2) / ((1.0 - f) * (1.0 - f) * N1), arma::norm(v.subvec(0, 1)) / N1); // preliminary value
|
|
double U = 0.1;
|
|
double oldU = 0.0;
|
|
int iterations = 0;
|
|
while (fabs(U - oldU) > 1.0E-4)
|
|
{
|
|
oldU = U;
|
|
N1 = c / sqrt(1.0 + ex2 * (cos(B) * cos(B)));
|
|
B = atan2(v(2) / ((1.0 - f) * (1.0 - f) * N1 + U), arma::norm(v.subvec(0, 1)) / (N1 + U));
|
|
U = arma::norm(v.subvec(0, 1)) / cos(B) - N1;
|
|
iterations = iterations + 1;
|
|
if (iterations > 100)
|
|
{
|
|
std::cout << "Failed to approximate U with desired precision. U-oldU:" << U - oldU << std::endl;
|
|
break;
|
|
}
|
|
}
|
|
// Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21)
|
|
double m0 = 0.0004;
|
|
double n = f / (2.0 - f);
|
|
double m = n * n * (1.0 / 4.0 + n * n / 64.0);
|
|
double w = (a * (-n - m0 + m * (1.0 - m0))) / (1.0 + n);
|
|
double Q_n = a + w;
|
|
|
|
// Easting and longitude of central meridian
|
|
double E0 = 500000.0;
|
|
double L0 = (zone - 30) * 6.0 - 3.0;
|
|
|
|
// Check tolerance for reverse transformation
|
|
// double tolutm = STRP_PI / 2.0 * 1.2e-10 * Q_n;
|
|
// double tolgeo = 0.000040;
|
|
// Coefficients of trigonometric series
|
|
//
|
|
// ellipsoidal to spherical geographical, KW p .186 --187, (51) - (52)
|
|
// bg[1] = n * (-2 + n * (2 / 3 + n * (4 / 3 + n * (-82 / 45))));
|
|
// bg[2] = n ^ 2 * (5 / 3 + n * (-16 / 15 + n * (-13 / 9)));
|
|
// bg[3] = n ^ 3 * (-26 / 15 + n * 34 / 21);
|
|
// bg[4] = n ^ 4 * 1237 / 630;
|
|
//
|
|
// spherical to ellipsoidal geographical, KW p.190 --191, (61) - (62) % gb[1] = n * (2 + n * (-2 / 3 + n * (-2 + n * 116 / 45)));
|
|
// gb[2] = n ^ 2 * (7 / 3 + n * (-8 / 5 + n * (-227 / 45)));
|
|
// gb[3] = n ^ 3 * (56 / 15 + n * (-136 / 35));
|
|
// gb[4] = n ^ 4 * 4279 / 630;
|
|
//
|
|
// spherical to ellipsoidal N, E, KW p.196, (69) % gtu[1] = n * (1 / 2 + n * (-2 / 3 + n * (5 / 16 + n * 41 / 180)));
|
|
// gtu[2] = n ^ 2 * (13 / 48 + n * (-3 / 5 + n * 557 / 1440));
|
|
// gtu[3] = n ^ 3 * (61 / 240 + n * (-103 / 140));
|
|
// gtu[4] = n ^ 4 * 49561 / 161280;
|
|
//
|
|
// ellipsoidal to spherical N, E, KW p.194, (65) % utg[1] = n * (-1 / 2 + n * (2 / 3 + n * (-37 / 96 + n * 1 / 360)));
|
|
// utg[2] = n ^ 2 * (-1 / 48 + n * (-1 / 15 + n * 437 / 1440));
|
|
// utg[3] = n ^ 3 * (-17 / 480 + n * 37 / 840);
|
|
// utg[4] = n ^ 4 * (-4397 / 161280);
|
|
//
|
|
// With f = 1 / 297 we get
|
|
|
|
arma::colvec bg = {-3.37077907e-3,
|
|
4.73444769e-6,
|
|
-8.29914570e-9,
|
|
1.58785330e-11};
|
|
|
|
arma::colvec gb = {3.37077588e-3,
|
|
6.62769080e-6,
|
|
1.78718601e-8,
|
|
5.49266312e-11};
|
|
|
|
arma::colvec gtu = {8.41275991e-4,
|
|
7.67306686e-7,
|
|
1.21291230e-9,
|
|
2.48508228e-12};
|
|
|
|
arma::colvec utg = {-8.41276339e-4,
|
|
-5.95619298e-8,
|
|
-1.69485209e-10,
|
|
-2.20473896e-13};
|
|
|
|
// Ellipsoidal latitude, longitude to spherical latitude, longitude
|
|
bool neg_geo = false;
|
|
|
|
if (B < 0.0) neg_geo = true;
|
|
|
|
double Bg_r = fabs(B);
|
|
double res_clensin = clsin(bg, 4, 2.0 * Bg_r);
|
|
Bg_r = Bg_r + res_clensin;
|
|
L0 = L0 * STRP_PI / 180.0;
|
|
double Lg_r = L - L0;
|
|
|
|
// Spherical latitude, longitude to complementary spherical latitude % i.e.spherical N, E
|
|
double cos_BN = cos(Bg_r);
|
|
double Np = atan2(sin(Bg_r), cos(Lg_r) * cos_BN);
|
|
double Ep = atanh(sin(Lg_r) * cos_BN);
|
|
|
|
// Spherical normalized N, E to ellipsoidal N, E
|
|
Np = 2.0 * Np;
|
|
Ep = 2.0 * Ep;
|
|
|
|
double dN;
|
|
double dE;
|
|
clksin(gtu, 4, Np, Ep, &dN, &dE);
|
|
Np = Np / 2.0;
|
|
Ep = Ep / 2.0;
|
|
Np = Np + dN;
|
|
Ep = Ep + dE;
|
|
double N = Q_n * Np;
|
|
double E = Q_n * Ep + E0;
|
|
if (neg_geo)
|
|
{
|
|
N = -N + 20000000.0;
|
|
}
|
|
r_enu(0) = E;
|
|
r_enu(1) = N;
|
|
r_enu(2) = U;
|
|
}
|
|
|
|
|
|
double clsin(const arma::colvec &ar, int degree, double argument)
|
|
{
|
|
// Clenshaw summation of sinus of argument.
|
|
//
|
|
// result = clsin(ar, degree, argument);
|
|
//
|
|
// Originally written in Matlab by Kai Borre
|
|
// Implemented in C++ by J.Arribas
|
|
|
|
double cos_arg = 2.0 * cos(argument);
|
|
double hr1 = 0.0;
|
|
double hr = 0.0;
|
|
double hr2;
|
|
for (int t = degree; t > 0; t--)
|
|
{
|
|
hr2 = hr1;
|
|
hr1 = hr;
|
|
hr = ar(t - 1) + cos_arg * hr1 - hr2;
|
|
}
|
|
|
|
return (hr * sin(argument));
|
|
}
|
|
|
|
|
|
void clksin(const arma::colvec &ar, int degree, double arg_real, double arg_imag, double *re, double *im)
|
|
{
|
|
// Clenshaw summation of sinus with complex argument
|
|
// [re, im] = clksin(ar, degree, arg_real, arg_imag);
|
|
//
|
|
// Originally written in Matlab by Kai Borre
|
|
// Implemented in C++ by J.Arribas
|
|
|
|
double sin_arg_r = sin(arg_real);
|
|
double cos_arg_r = cos(arg_real);
|
|
double sinh_arg_i = sinh(arg_imag);
|
|
double cosh_arg_i = cosh(arg_imag);
|
|
|
|
double r = 2.0 * cos_arg_r * cosh_arg_i;
|
|
double i = -2.0 * sin_arg_r * sinh_arg_i;
|
|
|
|
double hr1 = 0.0;
|
|
double hr = 0.0;
|
|
double hi1 = 0.0;
|
|
double hi = 0.0;
|
|
double hi2;
|
|
double hr2;
|
|
for (int t = degree; t > 0; t--)
|
|
{
|
|
hr2 = hr1;
|
|
hr1 = hr;
|
|
hi2 = hi1;
|
|
hi1 = hi;
|
|
double z = ar(t - 1) + r * hr1 - i * hi - hr2;
|
|
hi = i * hr1 + r * hi1 - hi2;
|
|
hr = z;
|
|
}
|
|
|
|
r = sin_arg_r * cosh_arg_i;
|
|
i = cos_arg_r * sinh_arg_i;
|
|
|
|
*re = r * hr - i * hi;
|
|
*im = r * hi + i * hr;
|
|
}
|
|
|
|
|
|
int findUtmZone(double latitude_deg, double longitude_deg)
|
|
{
|
|
// Function finds the UTM zone number for given longitude and latitude.
|
|
// The longitude value must be between -180 (180 degree West) and 180 (180
|
|
// degree East) degree. The latitude must be within -80 (80 degree South) and
|
|
// 84 (84 degree North).
|
|
//
|
|
// utmZone = findUtmZone(latitude, longitude);
|
|
//
|
|
// Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not
|
|
// 15 deg 30 min).
|
|
//
|
|
// Originally written in Matlab by Darius Plausinaitis
|
|
// Implemented in C++ by J.Arribas
|
|
|
|
// Check value bounds
|
|
if ((longitude_deg > 180.0) || (longitude_deg < -180.0))
|
|
std::cout << "Longitude value exceeds limits (-180:180).\n";
|
|
|
|
if ((latitude_deg > 84.0) || (latitude_deg < -80.0))
|
|
std::cout << "Latitude value exceeds limits (-80:84).\n";
|
|
|
|
//
|
|
// Find zone
|
|
//
|
|
|
|
// Start at 180 deg west = -180 deg
|
|
int utmZone = floor((180 + longitude_deg) / 6) + 1;
|
|
|
|
// Correct zone numbers for particular areas
|
|
if (latitude_deg > 72.0)
|
|
{
|
|
// Corrections for zones 31 33 35 37
|
|
if ((longitude_deg >= 0.0) && (longitude_deg < 9.0))
|
|
{
|
|
utmZone = 31;
|
|
}
|
|
else if ((longitude_deg >= 9.0) && (longitude_deg < 21.0))
|
|
{
|
|
utmZone = 33;
|
|
}
|
|
else if ((longitude_deg >= 21.0) && (longitude_deg < 33.0))
|
|
{
|
|
utmZone = 35;
|
|
}
|
|
else if ((longitude_deg >= 33.0) && (longitude_deg < 42.0))
|
|
{
|
|
utmZone = 37;
|
|
}
|
|
}
|
|
else if ((latitude_deg >= 56.0) && (latitude_deg < 64.0))
|
|
{
|
|
// Correction for zone 32
|
|
if ((longitude_deg >= 3.0) && (longitude_deg < 12.0))
|
|
utmZone = 32;
|
|
}
|
|
return utmZone;
|
|
}
|