mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-31 11:19:18 +00:00
Code Issues Releases Wiki Activity

Save one iteration in the Bancroft algorithm

Browse Source
This commit is contained in:
Carles Fernandez 2023-04-02 10:31:17 +02:00
parent 81eb2a07c3
commit 3def3c36cd
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
2 changed files with 60 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/algorithms/libs/rtklib/CMakeLists.txt
View File

@ -76,6 +76,12 @@ target_link_libraries(algorithms_libs_rtklib
BLAS::BLAS BLAS::BLAS
) )
if(ENABLE_ARMA_NO_DEBUG)
target_compile_definitions(algorithms_libs_rtklib
PRIVATE -DARMA_NO_BOUND_CHECKING=1
)
endif()
if(FILESYSTEM_FOUND) if(FILESYSTEM_FOUND)
target_compile_definitions(algorithms_libs_rtklib PUBLIC -DHAS_STD_FILESYSTEM=1) target_compile_definitions(algorithms_libs_rtklib PUBLIC -DHAS_STD_FILESYSTEM=1)
if(find_experimental) if(find_experimental)

113
src/algorithms/libs/rtklib/rtklib_pntpos.cc
View File

@ -29,6 +29,10 @@
* ----------------------------------------------------------------------------- * -----------------------------------------------------------------------------
*/ */
#if ARMA_NO_BOUND_CHECKING
#define ARMA_NO_DEBUG 1
#endif
#include "rtklib_pntpos.h" #include "rtklib_pntpos.h"
#include "rtklib_ephemeris.h" #include "rtklib_ephemeris.h"
#include "rtklib_ionex.h" #include "rtklib_ionex.h"
@ -615,70 +619,61 @@ arma::vec rough_bancroft(const arma::mat &B_pass)
{ {
const int m = B_pass.n_rows; const int m = B_pass.n_rows;
arma::vec pos = arma::zeros<arma::vec>(4); arma::vec pos = arma::zeros<arma::vec>(4);
for (int iter = 1; iter <= 2; iter++) arma::mat BBB;
bool success;
if (m > 4)
{
success = arma::pinv(BBB, B_pass);
}
else
{
success = arma::inv(BBB, B_pass);
}
if (!success)
{
return pos;
}
const arma::vec e = arma::ones<arma::vec>(m);
arma::vec alpha = arma::zeros<arma::vec>(m);
for (int i = 0; i < m; i++)
{
arma::vec Bi = B_pass.row(i).t();
alpha(i) = lorentz(Bi, Bi) / 2.0;
}
const arma::vec BBBe = BBB * e;
const arma::vec BBBalpha = BBB * alpha;
const double a = lorentz(BBBe, BBBe);
const double b = lorentz(BBBe, BBBalpha) - 1.0;
const double c = lorentz(BBBalpha, BBBalpha);
const double root = std::sqrt(b * b - a * c);
arma::vec r(2);
r(0) = (-b - root) / a;
r(1) = (-b + root) / a;
arma::mat possible_pos = arma::zeros<arma::mat>(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(2);
for (int j = 0; j < m; j++)
{ {
// We should rotate the matrix accounting for the travel time here,
// but for a rough first estimation we can skip it
arma::mat BBB;
bool success;
if (m > 4)
{
success = arma::inv(BBB, B_pass.t() * B_pass);
if (success)
{
BBB *= B_pass.t();
}
}
else
{
success = arma::inv(BBB, B_pass);
}
if (!success)
{
return pos;
}
const arma::vec e = arma::ones<arma::vec>(m);
arma::vec alpha = arma::zeros<arma::vec>(m);
for (int i = 0; i < m; i++)
{
arma::vec Bi = B_pass.row(i).t();
alpha(i) = lorentz(Bi, Bi) / 2.0;
}
const arma::vec BBBe = BBB * e;
const arma::vec BBBalpha = BBB * alpha;
const double a = lorentz(BBBe, BBBe);
const double b = lorentz(BBBe, BBBalpha) - 1.0;
const double c = lorentz(BBBalpha, BBBalpha);
const double root = std::sqrt(b * b - a * c);
arma::vec r(2);
r(0) = (-b - root) / a;
r(1) = (-b + root) / a;
arma::mat possible_pos = arma::zeros<arma::mat>(4, 2);
for (int i = 0; i < 2; i++) for (int i = 0; i < 2; i++)
{ {
possible_pos.col(i) = r(i) * BBBe + BBBalpha; const double c_dt = possible_pos(3, i);
possible_pos(3, i) = -possible_pos(3, i); const double calc = arma::norm(B_pass.row(j).head(3).t() - possible_pos.head_rows(3).col(i)) + c_dt;
} const double omc = B_pass(j, 3) - calc;
arma::vec abs_omc(2); abs_omc(i) = std::abs(omc);
for (int j = 0; j < m; j++)
{
for (int i = 0; i < 2; i++)
{
const double c_dt = possible_pos(3, i);
const double calc = arma::norm(B_pass.row(j).head(3).t() - possible_pos.head_rows(3).col(i)) + c_dt;
const double omc = B_pass(j, 3) - calc;
abs_omc(i) = std::abs(omc);
}
} }
}
if (abs_omc(0) > abs_omc(1)) if (abs_omc(0) > abs_omc(1))
{ {
pos = possible_pos.col(1); pos = possible_pos.col(1);
} }
else else
{ {
pos = possible_pos.col(0); pos = possible_pos.col(0);
}
} }
return pos; return pos;