2015-11-14 13:17:02 +00:00
|
|
|
/*!
|
2015-11-14 19:41:28 +00:00
|
|
|
* \file ls_pvt.cc
|
2015-11-14 13:17:02 +00:00
|
|
|
* \brief Implementation of a base class for Least Squares PVT solutions
|
|
|
|
* \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* -------------------------------------------------------------------------
|
|
|
|
*
|
|
|
|
* Copyright (C) 2010-2015 (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 <http://www.gnu.org/licenses/>.
|
|
|
|
*
|
|
|
|
* -------------------------------------------------------------------------
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "ls_pvt.h"
|
|
|
|
#include <exception>
|
|
|
|
#include "GPS_L1_CA.h"
|
|
|
|
#include <gflags/gflags.h>
|
|
|
|
#include <glog/logging.h>
|
|
|
|
|
|
|
|
|
|
|
|
using google::LogMessage;
|
|
|
|
|
|
|
|
|
2015-11-14 19:41:28 +00:00
|
|
|
Ls_Pvt::Ls_Pvt() : Pvt_Solution()
|
2015-11-14 13:17:02 +00:00
|
|
|
{
|
|
|
|
d_x_m = 0.0;
|
|
|
|
d_y_m = 0.0;
|
|
|
|
d_z_m = 0.0;
|
|
|
|
}
|
|
|
|
|
|
|
|
arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w)
|
|
|
|
{
|
|
|
|
/* Computes the Least Squares Solution.
|
|
|
|
* Inputs:
|
|
|
|
* satpos - Satellites positions in ECEF system: [X; Y; Z;]
|
|
|
|
* obs - Observations - the pseudorange measurements to each satellite
|
|
|
|
* w - weigths vector
|
|
|
|
*
|
|
|
|
* Returns:
|
|
|
|
* pos - receiver position and receiver clock error
|
|
|
|
* (in ECEF system: [X, Y, Z, dt])
|
|
|
|
*/
|
|
|
|
|
|
|
|
//=== Initialization =======================================================
|
|
|
|
int nmbOfIterations = 10; // TODO: include in config
|
|
|
|
int nmbOfSatellites;
|
|
|
|
nmbOfSatellites = satpos.n_cols; //Armadillo
|
|
|
|
arma::vec pos = "0.0 0.0 0.0 0.0";
|
|
|
|
arma::mat A;
|
|
|
|
arma::mat omc;
|
|
|
|
arma::mat az;
|
|
|
|
arma::mat el;
|
|
|
|
A = arma::zeros(nmbOfSatellites, 4);
|
|
|
|
omc = arma::zeros(nmbOfSatellites, 1);
|
|
|
|
az = arma::zeros(1, nmbOfSatellites);
|
|
|
|
el = arma::zeros(1, nmbOfSatellites);
|
|
|
|
arma::mat X = satpos;
|
|
|
|
arma::vec Rot_X;
|
|
|
|
double rho2;
|
|
|
|
double traveltime;
|
|
|
|
double trop = 0.0;
|
|
|
|
double dlambda;
|
|
|
|
double dphi;
|
|
|
|
double h;
|
|
|
|
arma::mat mat_tmp;
|
|
|
|
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) / GPS_C_m_s;
|
|
|
|
|
|
|
|
//--- Correct satellite position (do to earth rotation) --------
|
|
|
|
Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo
|
|
|
|
|
|
|
|
//--- Find DOA and range of satellites
|
|
|
|
Ls_Pvt::topocent(&d_visible_satellites_Az[i],
|
|
|
|
&d_visible_satellites_El[i],
|
|
|
|
&d_visible_satellites_Distance[i],
|
|
|
|
pos.subvec(0,2),
|
|
|
|
Rot_X - pos.subvec(0, 2));
|
|
|
|
if(traveltime < 0.1 && nmbOfSatellites > 3)
|
|
|
|
{
|
|
|
|
//--- Find receiver's height
|
|
|
|
Ls_Pvt::togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
|
|
|
|
|
|
|
|
//--- Find delay due to troposphere (in meters)
|
|
|
|
Ls_Pvt::tropo(&trop, sin(d_visible_satellites_El[i] * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
|
|
|
|
if(trop > 50.0 ) trop = 0.0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
//--- 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)
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
try
|
|
|
|
{
|
|
|
|
//-- compute the Dilution Of Precision values
|
|
|
|
d_Q = arma::inv(arma::htrans(A)*A);
|
|
|
|
}
|
|
|
|
catch(std::exception& e)
|
|
|
|
{
|
|
|
|
d_Q = arma::zeros(4,4);
|
|
|
|
}
|
|
|
|
return pos;
|
|
|
|
}
|