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

Make Bancroft work in other bands than L1, make the algorithm more stable in case of a singular matrix

Browse Source
This commit is contained in:
Carles Fernandez 2023-04-01 21:43:32 +02:00
parent 67c6dbcfda
commit 318a39bd79
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
1 changed files with 29 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -22,7 +22,7 @@
* ----------------------------------------------------------------------------- * -----------------------------------------------------------------------------
* Copyright (C) 2007-2013, T. Takasu * Copyright (C) 2007-2013, T. Takasu
* Copyright (C) 2017, Javier Arribas * Copyright (C) 2017, Javier Arribas
* Copyright (C) 2017, Carles Fernandez * Copyright (C) 2017-2023, Carles Fernandez
* All rights reserved. * All rights reserved.
* *
* SPDX-License-Identifier: BSD-2-Clause * SPDX-License-Identifier: BSD-2-Clause
@ -620,25 +620,34 @@ arma::vec rough_bancroft(const arma::mat &B_pass)
// We should rotate the matrix accounting for the travel time here, // We should rotate the matrix accounting for the travel time here,
// but for a rough first estimation we can skip it // but for a rough first estimation we can skip it
arma::mat BBB; arma::mat BBB;
bool success;
if (m > 4) if (m > 4)
{ {
BBB = arma::inv(B_pass.t() * B_pass) * B_pass.t(); success = arma::inv(BBB, B_pass.t() * B_pass);
if (success)
{
BBB *= B_pass.t();
}
} }
else else
{ {
BBB = arma::inv(B_pass); success = arma::inv(BBB, B_pass);
} }
arma::vec e = arma::ones<arma::vec>(m); if (!success)
{
return pos;
}
const arma::vec e = arma::ones<arma::vec>(m);
arma::vec alpha = arma::zeros<arma::vec>(m); arma::vec alpha = arma::zeros<arma::vec>(m);
for (int i = 0; i < m; i++) for (int i = 0; i < m; i++)
{ {
arma::vec Bi = B_pass.row(i).t(); arma::vec Bi = B_pass.row(i).t();
alpha(i) = lorentz(Bi, Bi) / 2; alpha(i) = lorentz(Bi, Bi) / 2.0;
} }
arma::vec BBBe = BBB * e; const arma::vec BBBe = BBB * e;
arma::vec BBBalpha = BBB * alpha; const arma::vec BBBalpha = BBB * alpha;
double a = lorentz(BBBe, BBBe); double a = lorentz(BBBe, BBBe);
double b = lorentz(BBBe, BBBalpha) - 1; double b = lorentz(BBBe, BBBalpha) - 1.0;
double c = lorentz(BBBalpha, BBBalpha); double c = lorentz(BBBalpha, BBBalpha);
double root = std::sqrt(b * b - a * c); double root = std::sqrt(b * b - a * c);
arma::vec r(2); arma::vec r(2);
@ -718,8 +727,19 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
B(i, 0) = rs[0 + i * 6]; B(i, 0) = rs[0 + i * 6];
B(i, 1) = rs[1 + i * 6]; B(i, 1) = rs[1 + i * 6];
B(i, 2) = rs[2 + i * 6]; B(i, 2) = rs[2 + i * 6];
if (obs[i].code[0] != CODE_NONE)
{
B(i, 3) = obs[i].P[0]; B(i, 3) = obs[i].P[0];
} }
else if (obs[i].code[1] != CODE_NONE)
{
B(i, 3) = obs[i].P[1];
}
else
{
B(i, 3) = obs[i].P[2];
}
}
arma::vec pos = rough_bancroft(B); arma::vec pos = rough_bancroft(B);
x[0] = pos(0); x[0] = pos(0);
x[1] = pos(1); x[1] = pos(1);