mirror of https://github.com/gnss-sdr/gnss-sdr
Fix initialization
This commit is contained in:
parent
eecfd10875
commit
6e65705b47
|
@ -188,7 +188,7 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
|
||||||
//=== Initialization =======================================================
|
//=== Initialization =======================================================
|
||||||
int nmbOfIterations = 10; // TODO: include in config
|
int nmbOfIterations = 10; // TODO: include in config
|
||||||
int nmbOfSatellites;
|
int nmbOfSatellites;
|
||||||
nmbOfSatellites = satpos.n_cols; //Armadillo
|
nmbOfSatellites = satpos.n_cols; // Armadillo
|
||||||
arma::mat w = arma::zeros(nmbOfSatellites, nmbOfSatellites);
|
arma::mat w = arma::zeros(nmbOfSatellites, nmbOfSatellites);
|
||||||
w.diag() = w_vec; //diagonal weight matrix
|
w.diag() = w_vec; //diagonal weight matrix
|
||||||
|
|
||||||
|
@ -196,12 +196,8 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
|
||||||
arma::vec pos = {rx_pos(0), rx_pos(1), rx_pos(2), 0}; // time error in METERS (time x speed)
|
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 A;
|
||||||
arma::mat omc;
|
arma::mat omc;
|
||||||
arma::mat az;
|
|
||||||
arma::mat el;
|
|
||||||
A = arma::zeros(nmbOfSatellites, 4);
|
A = arma::zeros(nmbOfSatellites, 4);
|
||||||
omc = arma::zeros(nmbOfSatellites, 1);
|
omc = arma::zeros(nmbOfSatellites, 1);
|
||||||
az = arma::zeros(1, nmbOfSatellites);
|
|
||||||
el = arma::zeros(1, nmbOfSatellites);
|
|
||||||
arma::mat X = satpos;
|
arma::mat X = satpos;
|
||||||
arma::vec Rot_X;
|
arma::vec Rot_X;
|
||||||
double rho2;
|
double rho2;
|
||||||
|
@ -210,7 +206,6 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
|
||||||
double dlambda;
|
double dlambda;
|
||||||
double dphi;
|
double dphi;
|
||||||
double h;
|
double h;
|
||||||
arma::mat mat_tmp;
|
|
||||||
arma::vec x;
|
arma::vec x;
|
||||||
|
|
||||||
//=== Iteratively find receiver position ===================================
|
//=== Iteratively find receiver position ===================================
|
||||||
|
@ -237,10 +232,12 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
|
||||||
Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo
|
Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo
|
||||||
|
|
||||||
//--- Find DOA and range of satellites
|
//--- Find DOA and range of satellites
|
||||||
double *az, *el, *dist;
|
double * azim = 0;
|
||||||
Ls_Pvt::topocent(az, el, dist, pos.subvec(0,2), Rot_X - pos.subvec(0, 2));
|
double * elev = 0;
|
||||||
this->set_visible_satellites_Az(i, *az);
|
double * dist = 0;
|
||||||
this->set_visible_satellites_El(i, *el);
|
Ls_Pvt::topocent(azim, elev, dist, pos.subvec(0,2), Rot_X - pos.subvec(0, 2));
|
||||||
|
this->set_visible_satellites_Az(i, *azim);
|
||||||
|
this->set_visible_satellites_El(i, *elev);
|
||||||
this->set_visible_satellites_Distance(i, *dist);
|
this->set_visible_satellites_Distance(i, *dist);
|
||||||
|
|
||||||
if(traveltime < 0.1 && nmbOfSatellites > 3)
|
if(traveltime < 0.1 && nmbOfSatellites > 3)
|
||||||
|
|
Loading…
Reference in New Issue