mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2026-10-03 22:31:49 +00:00
Code Issues Releases Wiki Activity

Code cleaning

Browse Source
This commit is contained in:
Carles Fernandez
2017-01-28 15:31:04 +01:00
parent d2c7bb62a1
commit 4438ffe916
10 changed files with 266 additions and 275 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc
+13 -20
View File
@@ -227,22 +227,22 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels,
this->set_msg_handler(pmt::mp("telemetry"),
boost::bind(&gps_l1_ca_pvt_cc::msg_handler_telemetry, this, _1));
//initialize kml_printer
// initialize kml_printer
std::string kml_dump_filename;
kml_dump_filename = d_dump_filename;
d_kml_printer = std::make_shared<Kml_Printer>();
d_kml_printer->set_headers(kml_dump_filename);
//initialize geojson_printer
// initialize geojson_printer
std::string geojson_dump_filename;
geojson_dump_filename = d_dump_filename;
d_geojson_printer = std::make_shared<GeoJSON_Printer>();
d_geojson_printer->set_headers(geojson_dump_filename);
//initialize nmea_printer
// initialize nmea_printer
d_nmea_printer = std::make_shared<Nmea_Printer>(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname);
//initialize rtcm_printer
// initialize rtcm_printer
std::string rtcm_dump_filename;
rtcm_dump_filename = d_dump_filename;
d_rtcm_tcp_port = rtcm_tcp_port;
@@ -330,7 +330,7 @@ void gps_l1_ca_pvt_cc::print_receiver_status(Gnss_Synchro** channels_synchroniza
d_last_status_print_seg = current_rx_seg;
std::cout << "Current input signal time = " << current_rx_seg << " [s]" << std::endl << std::flush;
//DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
// << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
// << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]";
}
}
@@ -340,7 +340,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items __attribute__((unused)), g
{
gnss_observables_map.clear();
d_sample_counter++;
Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; // Get the input pointer
print_receiver_status(in);
@@ -366,25 +366,18 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items __attribute__((unused)), g
if (gnss_observables_map.size() > 0 and d_ls_pvt->gps_ephemeris_map.size() > 0)
{
// compute on the fly PVT solution
//mod 8/4/2012 Set the PVT computation rate in this block
if ((d_sample_counter % d_output_rate_ms) == 0)
{
bool pvt_result;
pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, d_flag_averaging);
if (pvt_result == true)
{
//correct the observable to account for the receiver clock offset
for (std::map<int,Gnss_Synchro>::iterator it=gnss_observables_map.begin(); it!=gnss_observables_map.end(); ++it)
{
it->second.Pseudorange_m=it->second.Pseudorange_m-d_ls_pvt->d_rx_dt_s*GPS_C_m_s;
}
// send asynchronous message to observables block
// time offset is expressed as the equivalent travel distance [m]
//pmt::pmt_t value = pmt::from_double(d_ls_pvt->d_rx_dt_s);
//this->message_port_pub(pmt::mp("rx_dt_s"), value);
//std::cout<<"d_rx_dt_s*GPS_C_m_s="<<d_ls_pvt->d_rx_dt_s*GPS_C_m_s<<std::endl;
if( first_fix == true)
// correct the observable to account for the receiver clock offset
for (std::map<int,Gnss_Synchro>::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it)
{
it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->d_rx_dt_s * GPS_C_m_s;
}
if(first_fix == true)
{
std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
<< " UTC is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d
@@ -410,7 +403,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items __attribute__((unused)), g
b_rinex_header_written = true; // do not write header anymore
}
}
if(b_rinex_header_written) // Put here another condition to separate annotations (e.g 30 s)
if(b_rinex_header_written)
{
// Limit the RINEX navigation output rate to 1/6 seg
// Notice that d_sample_counter period is 1ms (for GPS correlators)
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
+33 -31
View File
@@ -81,14 +81,14 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter;
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
arma::vec W;//= arma::eye(valid_pseudoranges, valid_pseudoranges); //channels weights matrix
arma::vec obs;// = arma::zeros(valid_pseudoranges); // pseudoranges observation vector
arma::mat satpos;// = arma::zeros(3, valid_pseudoranges); //satellite positions matrix
arma::vec W; // channels weight vector
arma::vec obs; // pseudoranges observation vector
arma::mat satpos; // satellite positions matrix
int GPS_week = 0;
double utc = 0;
double utc = 0.0;
double TX_time_corrected_s;
double SV_clock_bias_s = 0;
double SV_clock_bias_s = 0.0;
d_flag_averaging = flag_averaging;
@@ -107,8 +107,8 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W.resize(valid_obs+1,1);
W(valid_obs)=1;
W.resize(valid_obs + 1, 1);
W(valid_obs) = 1;
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time
@@ -121,23 +121,24 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
// 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
satpos.resize(3,valid_obs+1);
satpos.resize(3, valid_obs + 1);
satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X;
satpos(1, valid_obs) = gps_ephemeris_iter->second.d_satpos_Y;
satpos(2, valid_obs) = gps_ephemeris_iter->second.d_satpos_Z;
// 4- fill the observations vector with the corrected pseudoranges
obs.resize(valid_obs+1,1);
obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s-d_rx_dt_s*GPS_C_m_s;
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s - d_rx_dt_s * GPS_C_m_s;
d_visible_satellites_IDs[valid_obs] = gps_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
valid_obs++;
// SV ECEF DEBUG OUTPUT
DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
<< " X=" << gps_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
<< " X=" << gps_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
// compute the UTC time for this SV (just to print the associated UTC timestamp)
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
@@ -162,25 +163,27 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
DLOG(INFO) << "obs=" << obs;
DLOG(INFO) << "W=" << W;
//check if this is the initial position computation
if (d_rx_dt_s==0)
{
//execute Bancroft's algorithm to estimate initial receiver position and time
std::cout<<"Executing Bancroft algorithm...\n";
rx_position_and_time =bancroftPos(satpos.t(), obs);
d_rx_pos=rx_position_and_time.rows(0,2); //save ECEF position for the next iteration
d_rx_dt_s=rx_position_and_time(3)/GPS_C_m_s; //save time for the next iteration [meters]->[seconds]
}
// check if this is the initial position computation
if (d_rx_dt_s == 0)
{
// execute Bancroft's algorithm to estimate initial receiver position and time
DLOG(INFO) << " Executing Bancroft algorithm...";
rx_position_and_time = bancroftPos(satpos.t(), obs);
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
d_rx_dt_s = rx_position_and_time(3) / GPS_C_m_s; // save time for the next iteration [meters]->[seconds]
}
//Execute WLS using previos position as the initialization point
// Execute WLS using previous position as the initialization point
rx_position_and_time = leastSquarePos(satpos, obs, W);
d_rx_pos=rx_position_and_time.rows(0,2); //save ECEF position for the next iteration
d_rx_dt_s+=rx_position_and_time(3)/GPS_C_m_s; //accumulate the rx time error for the next iteration [meters]->[seconds]
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds]
DLOG(INFO) << "(new)Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
DLOG(INFO) <<"Accumulated rx clock error="<<d_rx_dt_s<<" clock error for this iteration="<<rx_position_and_time(3)/GPS_C_m_s<<" [s]"<<std::endl;
DLOG(INFO) << "Accumulated rx clock error=" << d_rx_dt_s << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]";
cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
// Compute UTC time and print PVT solution
double secondsperweek = 604800.0; // number of seconds in one week (7*24*60*60)
boost::posix_time::time_duration t = boost::posix_time::seconds(utc + secondsperweek * static_cast<double>(GPS_week));
@@ -188,13 +191,12 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
d_position_UTC_time = p_time;
DLOG(INFO) << "Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
// ###### Compute DOPs ########
std::cout<<"c\r";
compute_DOP();
std::cout<<"d\r";
// ######## LOG FILE #########
if(d_flag_dump_enabled == true)
{
src/algorithms/PVT/libs/ls_pvt.cc
+124 -120
View File
@@ -44,135 +44,137 @@ Ls_Pvt::Ls_Pvt() : Pvt_Solution()
}
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, 56--59
// %Kai Borre 04-30-95, improved by C.C. Goad 11-24-96
// %Copyright (c) by Kai Borre
// %$Revision: 1.0 $ $Date: 1997/09/26 $
//
// % 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 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
// Copyright (c) by Kai Borre
// $Revision: 1.0 $ $Date: 1997/09/26 $
//
// 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_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)/GPS_C_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
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) / GPS_C_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);
}
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
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
//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
//
// %Kai Borre 04-22-95
// %Copyright (c) by Kai Borre
// %$Revision: 1.0 $ $Date: 1997/09/26 $
//
// % 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));
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 ob code by:
// Kai Borre 04-22-95
// Copyright (c) by Kai Borre
// $Revision: 1.0 $ $Date: 1997/09/26 $
//
// 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.
@@ -190,10 +192,10 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
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
arma::mat w = arma::zeros(nmbOfSatellites, nmbOfSatellites);
w.diag() = w_vec; //diagonal weight matrix
arma::vec pos = {{d_rx_pos(0)},{d_rx_pos(0)},{d_rx_pos(0)},0}; //time error in METERS (time x speed)
arma::vec pos = {d_rx_pos(0), d_rx_pos(0), d_rx_pos(0), 0}; // time error in METERS (time x speed)
arma::mat A;
arma::mat omc;
arma::mat az;
@@ -251,7 +253,9 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
{
//receiver is above the troposphere
trop = 0.0;
}else{
}
else
{
//--- 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 > 5.0 ) trop = 0.0; //check for erratic values
@@ -284,7 +288,7 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
try
{
//-- compute the Dilution Of Precision values
d_Q = arma::inv(arma::htrans(A)*A);
d_Q = arma::inv(arma::htrans(A) * A);
}
catch(std::exception& e)
{
src/algorithms/PVT/libs/pvt_solution.cc
+9 -9
View File
@@ -57,7 +57,7 @@ Pvt_Solution::Pvt_Solution()
b_valid_position = false;
d_averaging_depth = 0;
d_valid_observations = 0;
d_rx_pos=arma::zeros(3,1);
d_rx_pos = arma::zeros(3,1);
d_rx_dt_s = 0.0;
}
@@ -147,24 +147,24 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection)
int Pvt_Solution::togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z)
{
/* Subroutine to calculate geodetic coordinates latitude, longitude,
height given Cartesian coordinates X,Y,Z, and reference ellipsoid
values semi-major axis (a) and the inverse of flattening (finv).
height given Cartesian coordinates X,Y,Z, and reference ellipsoid
values semi-major axis (a) and the inverse of flattening (finv).
The output units of angular quantities will be in decimal degrees
(15.5 degrees not 15 deg 30 min). The output units of h will be the
same as the units of X,Y,Z,a.
The output units of angular quantities will be in decimal degrees
(15.5 degrees not 15 deg 30 min). The output units of h will be the
same as the units of X,Y,Z,a.
Inputs:
Inputs:
a - semi-major axis of the reference ellipsoid
finv - inverse of flattening of the reference ellipsoid
X,Y,Z - Cartesian coordinates
Outputs:
Outputs:
dphi - latitude
dlambda - longitude
h - height above reference ellipsoid
Based in a Matlab function by Kai Borre
Based in a Matlab function by Kai Borre
*/
*h = 0;