mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-11-05 09:36:23 +00:00
c178d9a8a6
Some API cleaning. The user does not need to call cart2geo anymore. Armadillo stuff moved to old ls_pvt solution
429 lines
16 KiB
C++
429 lines
16 KiB
C++
/*!
|
|
* \file ls_pvt.cc
|
|
* \brief Implementation of a base class for Least Squares PVT solutions
|
|
* \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es
|
|
*
|
|
*
|
|
* -------------------------------------------------------------------------
|
|
*
|
|
* Copyright (C) 2010-2019 (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.
|
|
*
|
|
* SPDX-License-Identifier: GPL-3.0-or-later
|
|
*
|
|
* -------------------------------------------------------------------------
|
|
*/
|
|
|
|
#include "ls_pvt.h"
|
|
#include "MATH_CONSTANTS.h"
|
|
#include "geofunctions.h"
|
|
#include <glog/logging.h>
|
|
#include <stdexcept>
|
|
|
|
arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs)
|
|
{
|
|
// BANCROFT Calculation of preliminary coordinates for a GPS receiver based on pseudoranges
|
|
// to 4 or more satellites. The ECEF coordinates are stored in satpos.
|
|
// The observed pseudoranges are stored in obs
|
|
// Reference: Bancroft, S. (1985) An Algebraic Solution of the GPS Equations,
|
|
// IEEE Trans. Aerosp. and Elec. Systems, AES-21, Issue 1, pp. 56--59
|
|
// Based on code by:
|
|
// Kai Borre 04-30-95, improved by C.C. Goad 11-24-96
|
|
//
|
|
// Test values to use in debugging
|
|
// B_pass =[ -11716227.778 -10118754.628 21741083.973 22163882.029;
|
|
// -12082643.974 -20428242.179 11741374.154 21492579.823;
|
|
// 14373286.650 -10448439.349 19596404.858 21492492.771;
|
|
// 10278432.244 -21116508.618 -12689101.970 25284588.982 ];
|
|
// Solution: 595025.053 -4856501.221 4078329.981
|
|
//
|
|
// Test values to use in debugging
|
|
// B_pass = [14177509.188 -18814750.650 12243944.449 21119263.116;
|
|
// 15097198.146 -4636098.555 21326705.426 22527063.486;
|
|
// 23460341.997 -9433577.991 8174873.599 23674159.579;
|
|
// -8206498.071 -18217989.839 17605227.065 20951643.862;
|
|
// 1399135.830 -17563786.820 19705534.862 20155386.649;
|
|
// 6995655.459 -23537808.269 -9927906.485 24222112.972 ];
|
|
// Solution: 596902.683 -4847843.316 4088216.740
|
|
|
|
arma::vec pos = arma::zeros(4, 1);
|
|
arma::mat B_pass = arma::zeros(obs.size(), 4);
|
|
B_pass.submat(0, 0, obs.size() - 1, 2) = satpos;
|
|
B_pass.col(3) = obs;
|
|
|
|
arma::mat B;
|
|
arma::mat BBB;
|
|
double traveltime = 0;
|
|
for (int iter = 0; iter < 2; iter++)
|
|
{
|
|
B = B_pass;
|
|
int m = arma::size(B, 0);
|
|
for (int i = 0; i < m; i++)
|
|
{
|
|
int x = B(i, 0);
|
|
int y = B(i, 1);
|
|
if (iter == 0)
|
|
{
|
|
traveltime = 0.072;
|
|
}
|
|
else
|
|
{
|
|
int z = B(i, 2);
|
|
double rho = (x - pos(0)) * (x - pos(0)) + (y - pos(1)) * (y - pos(1)) + (z - pos(2)) * (z - pos(2));
|
|
traveltime = sqrt(rho) / SPEED_OF_LIGHT_M_S;
|
|
}
|
|
double angle = traveltime * 7.292115147e-5;
|
|
double cosa = cos(angle);
|
|
double sina = sin(angle);
|
|
B(i, 0) = cosa * x + sina * y;
|
|
B(i, 1) = -sina * x + cosa * y;
|
|
} // % i-loop
|
|
|
|
if (m > 3)
|
|
{
|
|
BBB = arma::inv(B.t() * B) * B.t();
|
|
}
|
|
else
|
|
{
|
|
BBB = arma::inv(B);
|
|
}
|
|
arma::vec e = arma::ones(m, 1);
|
|
arma::vec alpha = arma::zeros(m, 1);
|
|
for (int i = 0; i < m; i++)
|
|
{
|
|
alpha(i) = lorentz(B.row(i).t(), B.row(i).t()) / 2.0;
|
|
}
|
|
arma::mat BBBe = BBB * e;
|
|
arma::mat BBBalpha = BBB * alpha;
|
|
double a = lorentz(BBBe, BBBe);
|
|
double b = lorentz(BBBe, BBBalpha) - 1;
|
|
double c = lorentz(BBBalpha, BBBalpha);
|
|
double root = sqrt(b * b - a * c);
|
|
arma::vec r = {(-b - root) / a, (-b + root) / a};
|
|
arma::mat possible_pos = arma::zeros(4, 2);
|
|
for (int i = 0; i < 2; i++)
|
|
{
|
|
possible_pos.col(i) = r(i) * BBBe + BBBalpha;
|
|
possible_pos(3, i) = -possible_pos(3, i);
|
|
}
|
|
|
|
arma::vec abs_omc = arma::zeros(2, 1);
|
|
for (int j = 0; j < m; j++)
|
|
{
|
|
for (int i = 0; i < 2; i++)
|
|
{
|
|
double c_dt = possible_pos(3, i);
|
|
double calc = arma::norm(satpos.row(i).t() - possible_pos.col(i).rows(0, 2)) + c_dt;
|
|
double omc = obs(j) - calc;
|
|
abs_omc(i) = std::abs(omc);
|
|
}
|
|
} // % j-loop
|
|
|
|
// discrimination between roots
|
|
if (abs_omc(0) > abs_omc(1))
|
|
{
|
|
pos = possible_pos.col(1);
|
|
}
|
|
else
|
|
{
|
|
pos = possible_pos.col(0);
|
|
}
|
|
} // % iter loop
|
|
return pos;
|
|
}
|
|
|
|
|
|
double Ls_Pvt::lorentz(const arma::vec& x, const arma::vec& y)
|
|
{
|
|
// LORENTZ Calculates the Lorentz inner product of the two
|
|
// 4 by 1 vectors x and y
|
|
// Based on code by:
|
|
// Kai Borre 04-22-95
|
|
//
|
|
// M = diag([1 1 1 -1]);
|
|
// p = x'*M*y;
|
|
|
|
return (x(0) * y(0) + x(1) * y(1) + x(2) * y(2) - x(3) * y(3));
|
|
}
|
|
|
|
|
|
arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs, const arma::vec& w_vec)
|
|
{
|
|
/* Computes the Least Squares Solution.
|
|
* Inputs:
|
|
* satpos - Satellites positions in ECEF system: [X; Y; Z;]
|
|
* obs - Observations - the pseudorange measurements to each satellite
|
|
* w - weight vector
|
|
*
|
|
* Returns:
|
|
* pos - receiver position and receiver clock error
|
|
* (in ECEF system: [X, Y, Z, dt])
|
|
*/
|
|
|
|
//=== Initialization =======================================================
|
|
constexpr double GPS_STARTOFFSET_MS = 68.802; // [ms] Initial signal travel time
|
|
int nmbOfIterations = 10; // TODO: include in config
|
|
int nmbOfSatellites;
|
|
nmbOfSatellites = satpos.n_cols; // Armadillo
|
|
arma::mat w = arma::zeros(nmbOfSatellites, nmbOfSatellites);
|
|
w.diag() = w_vec; // diagonal weight matrix
|
|
|
|
std::array<double, 3> rx_pos = this->get_rx_pos();
|
|
arma::vec pos = {rx_pos[0], rx_pos[1], rx_pos[2], 0}; // time error in METERS (time x speed)
|
|
arma::mat A;
|
|
arma::mat omc;
|
|
A = arma::zeros(nmbOfSatellites, 4);
|
|
omc = arma::zeros(nmbOfSatellites, 1);
|
|
arma::mat X = satpos;
|
|
arma::vec Rot_X;
|
|
double rho2;
|
|
double traveltime;
|
|
double trop = 0.0;
|
|
double dlambda;
|
|
double dphi;
|
|
double h;
|
|
arma::vec x;
|
|
|
|
//=== Iteratively find receiver position ===================================
|
|
for (int iter = 0; iter < nmbOfIterations; iter++)
|
|
{
|
|
for (int i = 0; i < nmbOfSatellites; i++)
|
|
{
|
|
if (iter == 0)
|
|
{
|
|
// --- Initialize variables at the first iteration -------------
|
|
Rot_X = X.col(i); // Armadillo
|
|
trop = 0.0;
|
|
}
|
|
else
|
|
{
|
|
// --- Update equations ----------------------------------------
|
|
rho2 = (X(0, i) - pos(0)) *
|
|
(X(0, i) - pos(0)) +
|
|
(X(1, i) - pos(1)) *
|
|
(X(1, i) - pos(1)) +
|
|
(X(2, i) - pos(2)) *
|
|
(X(2, i) - pos(2));
|
|
traveltime = sqrt(rho2) / SPEED_OF_LIGHT_M_S;
|
|
|
|
// --- Correct satellite position (do to earth rotation) -------
|
|
std::array<double, 3> rot_x = Ls_Pvt::rotateSatellite(traveltime, {X(0, i), X(1, i), X(2, i)});
|
|
Rot_X = {rot_x[0], rot_x[1], rot_x[2]};
|
|
|
|
// -- Find DOA and range of satellites
|
|
double* azim = nullptr;
|
|
double* elev = nullptr;
|
|
double* dist = nullptr;
|
|
topocent(azim, elev, dist, pos.subvec(0, 2), Rot_X - pos.subvec(0, 2));
|
|
|
|
if (traveltime < 0.1 && nmbOfSatellites > 3)
|
|
{
|
|
// --- Find receiver's height
|
|
togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
|
|
// Add troposphere correction if the receiver is below the troposphere
|
|
if (h > 15000)
|
|
{
|
|
// receiver is above the troposphere
|
|
trop = 0.0;
|
|
}
|
|
else
|
|
{
|
|
// --- Find delay due to troposphere (in meters)
|
|
Ls_Pvt::tropo(&trop, sin(elev[0] * GNSS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
|
|
if (trop > 5.0)
|
|
{
|
|
trop = 0.0; // check for erratic values
|
|
}
|
|
}
|
|
}
|
|
}
|
|
// --- Apply the corrections ----------------------------------------
|
|
omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo
|
|
|
|
// -- Construct the A matrix ---------------------------------------
|
|
// Armadillo
|
|
A(i, 0) = (-(Rot_X(0) - pos(0))) / obs(i);
|
|
A(i, 1) = (-(Rot_X(1) - pos(1))) / obs(i);
|
|
A(i, 2) = (-(Rot_X(2) - pos(2))) / obs(i);
|
|
A(i, 3) = 1.0;
|
|
}
|
|
|
|
// -- Find position update ---------------------------------------------
|
|
x = arma::solve(w * A, w * omc); // Armadillo
|
|
|
|
// -- Apply position update --------------------------------------------
|
|
pos = pos + x;
|
|
if (arma::norm(x, 2) < 1e-4)
|
|
{
|
|
break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm)
|
|
}
|
|
}
|
|
|
|
// check the consistency of the PVT solution
|
|
if (((fabs(pos(3)) * 1000.0) / SPEED_OF_LIGHT_M_S) > GPS_STARTOFFSET_MS * 2)
|
|
{
|
|
LOG(WARNING) << "Receiver time offset out of range! Estimated RX Time error [s]:" << pos(3) / SPEED_OF_LIGHT_M_S;
|
|
throw std::runtime_error("Receiver time offset out of range!");
|
|
}
|
|
return pos;
|
|
}
|
|
|
|
|
|
int Ls_Pvt::tropo(double* ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km)
|
|
{
|
|
/* Inputs:
|
|
sinel - sin of elevation angle of satellite
|
|
hsta_km - height of station in km
|
|
p_mb - atmospheric pressure in mb at height hp_km
|
|
t_kel - surface temperature in degrees Kelvin at height htkel_km
|
|
hum - humidity in % at height hhum_km
|
|
hp_km - height of pressure measurement in km
|
|
htkel_km - height of temperature measurement in km
|
|
hhum_km - height of humidity measurement in km
|
|
|
|
Outputs:
|
|
ddr_m - range correction (meters)
|
|
|
|
Reference
|
|
Goad, C.C. & Goodman, L. (1974) A Modified Hopfield Tropospheric
|
|
Refraction Correction Model. Paper presented at the
|
|
American Geophysical Union Annual Fall Meeting, San
|
|
Francisco, December 12-17
|
|
|
|
Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre
|
|
*/
|
|
|
|
const double a_e = 6378.137; // semi-major axis of earth ellipsoid
|
|
const double b0 = 7.839257e-5;
|
|
const double tlapse = -6.5;
|
|
const double em = -978.77 / (2.8704e6 * tlapse * 1.0e-5);
|
|
const double tkhum = t_kel + tlapse * (hhum_km - htkel_km);
|
|
const double atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15);
|
|
const double e0 = 0.0611 * hum * pow(10, atkel);
|
|
const double tksea = t_kel - tlapse * htkel_km;
|
|
const double tkelh = tksea + tlapse * hhum_km;
|
|
const double e0sea = e0 * pow((tksea / tkelh), (4 * em));
|
|
const double tkelp = tksea + tlapse * hp_km;
|
|
const double psea = p_mb * pow((tksea / tkelp), em);
|
|
|
|
if (sinel < 0)
|
|
{
|
|
sinel = 0.0;
|
|
}
|
|
|
|
double tropo_delay = 0.0;
|
|
bool done = false;
|
|
double refsea = 77.624e-6 / tksea;
|
|
double htop = 1.1385e-5 / refsea;
|
|
refsea = refsea * psea;
|
|
double ref = refsea * pow(((htop - hsta_km) / htop), 4);
|
|
|
|
double a;
|
|
double b;
|
|
double rtop;
|
|
|
|
while (true)
|
|
{
|
|
rtop = pow((a_e + htop), 2) - pow((a_e + hsta_km), 2) * (1 - pow(sinel, 2));
|
|
|
|
// check to see if geometry is crazy
|
|
if (rtop < 0)
|
|
{
|
|
rtop = 0;
|
|
}
|
|
|
|
rtop = sqrt(rtop) - (a_e + hsta_km) * sinel;
|
|
|
|
a = -sinel / (htop - hsta_km);
|
|
b = -b0 * (1 - pow(sinel, 2)) / (htop - hsta_km);
|
|
|
|
arma::vec rn = arma::vec(8);
|
|
rn.zeros();
|
|
|
|
for (int i = 0; i < 8; i++)
|
|
{
|
|
rn(i) = pow(rtop, (i + 1 + 1));
|
|
}
|
|
|
|
arma::rowvec alpha = {2 * a, 2 * pow(a, 2) + 4 * b / 3, a * (pow(a, 2) + 3 * b),
|
|
pow(a, 4) / 5 + 2.4 * pow(a, 2) * b + 1.2 * pow(b, 2), 2 * a * b * (pow(a, 2) + 3 * b) / 3,
|
|
pow(b, 2) * (6 * pow(a, 2) + 4 * b) * 1.428571e-1, 0, 0};
|
|
|
|
if (pow(b, 2) > 1.0e-35)
|
|
{
|
|
alpha(6) = a * pow(b, 3) / 2;
|
|
alpha(7) = pow(b, 4) / 9;
|
|
}
|
|
|
|
double dr = rtop;
|
|
arma::mat aux_ = alpha * rn;
|
|
dr = dr + aux_(0, 0);
|
|
tropo_delay = tropo_delay + dr * ref * 1000;
|
|
|
|
if (done == true)
|
|
{
|
|
*ddr_m = tropo_delay;
|
|
break;
|
|
}
|
|
|
|
done = true;
|
|
refsea = (371900.0e-6 / tksea - 12.92e-6) / tksea;
|
|
htop = 1.1385e-5 * (1255 / tksea + 0.05) / refsea;
|
|
ref = refsea * e0sea * pow(((htop - hsta_km) / htop), 4);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
std::array<double, 3> Ls_Pvt::rotateSatellite(double traveltime, const std::array<double, 3>& X_sat)
|
|
{
|
|
/*
|
|
* Returns rotated satellite ECEF coordinates due to Earth
|
|
* rotation during signal travel time
|
|
*
|
|
* Inputs:
|
|
* travelTime - signal travel time
|
|
* X_sat - satellite's ECEF coordinates
|
|
*
|
|
* Returns:
|
|
* X_sat_rot - rotated satellite's coordinates (ECEF)
|
|
*/
|
|
|
|
const double omegatau = GNSS_OMEGA_EARTH_DOT * traveltime;
|
|
const double cosomg = cos(omegatau);
|
|
const double sinomg = sin(omegatau);
|
|
const double x = cosomg * X_sat[0] + sinomg * X_sat[1];
|
|
const double y = -sinomg * X_sat[0] + cosomg * X_sat[1];
|
|
|
|
std::array<double, 3> X_sat_rot = {x, y, X_sat[2]};
|
|
return X_sat_rot;
|
|
}
|
|
|
|
|
|
double Ls_Pvt::get_gdop() const
|
|
{
|
|
return 0.0; // not implemented
|
|
}
|
|
|
|
|
|
double Ls_Pvt::get_pdop() const
|
|
{
|
|
return 0.0; // not implemented
|
|
}
|
|
|
|
|
|
double Ls_Pvt::get_hdop() const
|
|
{
|
|
return 0.0; // not implemented
|
|
}
|
|
|
|
|
|
double Ls_Pvt::get_vdop() const
|
|
{
|
|
return 0.0; // not implemented
|
|
}
|