mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-11-15 14:25:00 +00:00
Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into fpga
This commit is contained in:
Marc Majoral
commit
17ddab1c3e
@ -24,10 +24,10 @@
|
||||
# also defined, but not for general use are
|
||||
# GPSTK_LIBRARY, where to find the GPSTK library.
|
||||
|
||||
FIND_PATH(GPSTK_INCLUDE_DIR Rinex3ObsBase.hpp
|
||||
HINTS /usr/include/gpstk
|
||||
/usr/local/include/gpstk
|
||||
/opt/local/include/gpstk )
|
||||
FIND_PATH(GPSTK_INCLUDE_DIR gpstk/Rinex3ObsBase.hpp
|
||||
HINTS /usr/include
|
||||
/usr/local/include
|
||||
/opt/local/include )
|
||||
|
||||
SET(GPSTK_NAMES ${GPSTK_NAMES} gpstk libgpstk)
|
||||
FIND_LIBRARY(GPSTK_LIBRARY NAMES ${GPSTK_NAMES}
|
||||
|
||||
@ -246,7 +246,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
|
||||
else if (obs->code[i] != CODE_NONE and obs->code[j] == CODE_NONE)
|
||||
{
|
||||
P1 += P1_C1; /* C1->P1 */
|
||||
PC = P1 + P1_P2;
|
||||
PC = P1 - P1_P2;
|
||||
}
|
||||
else if (obs->code[i] == CODE_NONE and obs->code[j] != CODE_NONE)
|
||||
{
|
||||
|
||||
@ -0,0 +1,122 @@
|
||||
/*!
|
||||
* \file volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn.h
|
||||
* \brief VOLK_GNSSSDR kernel: multiplies N complex (32-bit float per component) vectors
|
||||
* by a common vector, phase rotated and accumulates the results in N float complex outputs.
|
||||
* \authors <ul>
|
||||
* <li> Antonio Ramos 2018. antonio.ramosdet(at)gmail.com
|
||||
* </ul>
|
||||
*
|
||||
* VOLK_GNSSSDR kernel that multiplies N 32 bits complex vectors by a common vector, which is
|
||||
* phase-rotated by phase offset and phase increment, and accumulates the results
|
||||
* in N 32 bits float complex outputs.
|
||||
* It is optimized to perform the N tap correlation process in GNSS receivers.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (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 <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \page volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn
|
||||
*
|
||||
* \b Overview
|
||||
*
|
||||
* Rotates and multiplies the reference complex vector with an arbitrary number of other real vectors,
|
||||
* accumulates the results and stores them in the output vector.
|
||||
* The rotation is done at a fixed rate per sample, from an initial \p phase offset.
|
||||
* This function can be used for Doppler wipe-off and multiple correlator.
|
||||
*
|
||||
* <b>Dispatcher Prototype</b>
|
||||
* \code
|
||||
* void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn(lv_32fc_t* result, const lv_32fc_t* in_common, const lv_32fc_t phase_inc, const lv_32fc_t phase_inc_rate, lv_32fc_t* phase, const float** in_a, int num_a_vectors, unsigned int num_points);
|
||||
* \endcode
|
||||
*
|
||||
* \b Inputs
|
||||
* \li in_common: Pointer to one of the vectors to be rotated, multiplied and accumulated (reference vector).
|
||||
* \li phase_inc: Phase increment = lv_cmake(cos(phase_step_rad), sin(phase_step_rad))
|
||||
* \li phase_inc_rate: Phase increment rate = lv_cmake(cos(phase_step_rate_rad), sin(phase_step_rate_rad))
|
||||
* \li phase: Initial phase = lv_cmake(cos(initial_phase_rad), sin(initial_phase_rad))
|
||||
* \li in_a: Pointer to an array of pointers to multiple vectors to be multiplied and accumulated.
|
||||
* \li num_a_vectors: Number of vectors to be multiplied by the reference vector and accumulated.
|
||||
* \li num_points: Number of complex values to be multiplied together, accumulated and stored into \p result.
|
||||
*
|
||||
* \b Outputs
|
||||
* \li phase: Final phase.
|
||||
* \li result: Vector of \p num_a_vectors components with the multiple vectors of \p in_a rotated, multiplied by \p in_common and accumulated.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_H
|
||||
#define INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_H
|
||||
|
||||
|
||||
#include <volk_gnsssdr/volk_gnsssdr.h>
|
||||
#include <volk_gnsssdr/volk_gnsssdr_malloc.h>
|
||||
#include <volk_gnsssdr/volk_gnsssdr_complex.h>
|
||||
#include <volk_gnsssdr/saturation_arithmetic.h>
|
||||
#include <math.h>
|
||||
|
||||
#ifdef LV_HAVE_GENERIC
|
||||
|
||||
static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_generic(lv_32fc_t* result, const lv_32fc_t* in_common, const lv_32fc_t phase_inc, const lv_32fc_t phase_inc_rate, lv_32fc_t* phase, const float** in_a, int num_a_vectors, unsigned int num_points)
|
||||
{
|
||||
lv_32fc_t tmp32_1;
|
||||
#ifdef __cplusplus
|
||||
lv_32fc_t half_phase_inc_rate = std::sqrt(phase_inc_rate);
|
||||
#else
|
||||
lv_32fc_t half_phase_inc_rate = csqrtf(phase_inc_rate);
|
||||
#endif
|
||||
lv_32fc_t constant_rotation = phase_inc * half_phase_inc_rate;
|
||||
lv_32fc_t delta_phase_rate = lv_cmake(1.0f, 0.0f);
|
||||
int n_vec;
|
||||
unsigned int n;
|
||||
for (n_vec = 0; n_vec < num_a_vectors; n_vec++)
|
||||
{
|
||||
result[n_vec] = lv_cmake(0.0f, 0.0f);
|
||||
}
|
||||
for (n = 0; n < num_points; n++)
|
||||
{
|
||||
tmp32_1 = *in_common++ * (*phase);
|
||||
// Regenerate phase
|
||||
if (n % 256 == 0)
|
||||
{
|
||||
#ifdef __cplusplus
|
||||
(*phase) /= std::abs((*phase));
|
||||
delta_phase_rate /= std::abs(delta_phase_rate);
|
||||
#else
|
||||
(*phase) /= hypotf(lv_creal(*phase), lv_cimag(*phase));
|
||||
delta_phase_rate /= hypotf(lv_creal(delta_phase_rate), lv_cimag(delta_phase_rate));
|
||||
#endif
|
||||
}
|
||||
(*phase) *= (constant_rotation * delta_phase_rate);
|
||||
delta_phase_rate *= phase_inc_rate;
|
||||
for (n_vec = 0; n_vec < num_a_vectors; n_vec++)
|
||||
{
|
||||
result[n_vec] += (tmp32_1 * in_a[n_vec][n]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif /*LV_HAVE_GENERIC*/
|
||||
|
||||
#endif /* INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_H */
|
||||
@ -0,0 +1,163 @@
|
||||
/*!
|
||||
* \file volk_gnsssdr_32fc_32f_rotator_dotprodxnpuppet_32fc.h
|
||||
* \brief Volk puppet for the multiple 16-bit complex dot product kernel.
|
||||
* \authors <ul>
|
||||
* <li> Carles Fernandez Prades 2016 cfernandez at cttc dot cat
|
||||
* </ul>
|
||||
*
|
||||
* Volk puppet for integrating the resampler into volk's test system
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (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 <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc_H
|
||||
#define INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc_H
|
||||
|
||||
#include "volk_gnsssdr/volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn.h"
|
||||
#include <volk_gnsssdr/volk_gnsssdr_malloc.h>
|
||||
#include <volk_gnsssdr/volk_gnsssdr.h>
|
||||
#include <string.h>
|
||||
|
||||
#ifdef LV_HAVE_GENERIC
|
||||
|
||||
static inline void volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc_generic(lv_32fc_t* result, const lv_32fc_t* local_code, const float* in, unsigned int num_points)
|
||||
{
|
||||
// phases must be normalized. Phase rotator expects a complex exponential input!
|
||||
float rem_carrier_phase_in_rad = 0.25;
|
||||
float phase_step_rad = 0.1;
|
||||
lv_32fc_t phase[1];
|
||||
phase[0] = lv_cmake(cos(rem_carrier_phase_in_rad), sin(rem_carrier_phase_in_rad));
|
||||
lv_32fc_t phase_inc[1];
|
||||
phase_inc[0] = lv_cmake(cos(phase_step_rad), sin(phase_step_rad));
|
||||
lv_32fc_t phase_inc_rate[1];
|
||||
phase_inc_rate[0] = lv_cmake(cos(phase_step_rad * 0.001), sin(phase_step_rad * 0.001));
|
||||
int n;
|
||||
int num_a_vectors = 3;
|
||||
float** in_a = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_a_vectors, volk_gnsssdr_get_alignment());
|
||||
for (n = 0; n < num_a_vectors; n++)
|
||||
{
|
||||
in_a[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
|
||||
memcpy((float*)in_a[n], (float*)in, sizeof(float) * num_points);
|
||||
}
|
||||
volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn_generic(result, local_code, phase_inc[0], phase_inc_rate[0], phase, (const float**)in_a, num_a_vectors, num_points);
|
||||
|
||||
for (n = 0; n < num_a_vectors; n++)
|
||||
{
|
||||
volk_gnsssdr_free(in_a[n]);
|
||||
}
|
||||
volk_gnsssdr_free(in_a);
|
||||
}
|
||||
#endif // Generic
|
||||
|
||||
//
|
||||
//#ifdef LV_HAVE_GENERIC
|
||||
//static inline void volk_gnsssdr_32fc_32f_rotator_dotprodxnpuppet_32fc_generic_reload(lv_32fc_t* result, const lv_32fc_t* local_code, const float* in, unsigned int num_points)
|
||||
//{
|
||||
// // phases must be normalized. Phase rotator expects a complex exponential input!
|
||||
// float rem_carrier_phase_in_rad = 0.25;
|
||||
// float phase_step_rad = 0.1;
|
||||
// lv_32fc_t phase[1];
|
||||
// phase[0] = lv_cmake(cos(rem_carrier_phase_in_rad), sin(rem_carrier_phase_in_rad));
|
||||
// lv_32fc_t phase_inc[1];
|
||||
// phase_inc[0] = lv_cmake(cos(phase_step_rad), sin(phase_step_rad));
|
||||
// int n;
|
||||
// int num_a_vectors = 3;
|
||||
// float** in_a = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_a_vectors, volk_gnsssdr_get_alignment());
|
||||
// for (n = 0; n < num_a_vectors; n++)
|
||||
// {
|
||||
// in_a[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
|
||||
// memcpy((float*)in_a[n], (float*)in, sizeof(float) * num_points);
|
||||
// }
|
||||
// volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn_generic_reload(result, local_code, phase_inc[0], phase, (const float**)in_a, num_a_vectors, num_points);
|
||||
//
|
||||
// for (n = 0; n < num_a_vectors; n++)
|
||||
// {
|
||||
// volk_gnsssdr_free(in_a[n]);
|
||||
// }
|
||||
// volk_gnsssdr_free(in_a);
|
||||
//}
|
||||
//
|
||||
//#endif // Generic
|
||||
//
|
||||
//#ifdef LV_HAVE_AVX
|
||||
//static inline void volk_gnsssdr_32fc_32f_rotator_dotprodxnpuppet_32fc_u_avx(lv_32fc_t* result, const lv_32fc_t* local_code, const float* in, unsigned int num_points)
|
||||
//{
|
||||
// // phases must be normalized. Phase rotator expects a complex exponential input!
|
||||
// float rem_carrier_phase_in_rad = 0.25;
|
||||
// float phase_step_rad = 0.1;
|
||||
// lv_32fc_t phase[1];
|
||||
// phase[0] = lv_cmake(cos(rem_carrier_phase_in_rad), sin(rem_carrier_phase_in_rad));
|
||||
// lv_32fc_t phase_inc[1];
|
||||
// phase_inc[0] = lv_cmake(cos(phase_step_rad), sin(phase_step_rad));
|
||||
// int n;
|
||||
// int num_a_vectors = 3;
|
||||
// float** in_a = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_a_vectors, volk_gnsssdr_get_alignment());
|
||||
// for (n = 0; n < num_a_vectors; n++)
|
||||
// {
|
||||
// in_a[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
|
||||
// memcpy((float*)in_a[n], (float*)in, sizeof(float) * num_points);
|
||||
// }
|
||||
// volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn_u_avx(result, local_code, phase_inc[0], phase, (const float**)in_a, num_a_vectors, num_points);
|
||||
//
|
||||
// for (n = 0; n < num_a_vectors; n++)
|
||||
// {
|
||||
// volk_gnsssdr_free(in_a[n]);
|
||||
// }
|
||||
// volk_gnsssdr_free(in_a);
|
||||
//}
|
||||
//
|
||||
//#endif // AVX
|
||||
//
|
||||
//
|
||||
//#ifdef LV_HAVE_AVX
|
||||
//static inline void volk_gnsssdr_32fc_32f_rotator_dotprodxnpuppet_32fc_a_avx(lv_32fc_t* result, const lv_32fc_t* local_code, const float* in, unsigned int num_points)
|
||||
//{
|
||||
// // phases must be normalized. Phase rotator expects a complex exponential input!
|
||||
// float rem_carrier_phase_in_rad = 0.25;
|
||||
// float phase_step_rad = 0.1;
|
||||
// lv_32fc_t phase[1];
|
||||
// phase[0] = lv_cmake(cos(rem_carrier_phase_in_rad), sin(rem_carrier_phase_in_rad));
|
||||
// lv_32fc_t phase_inc[1];
|
||||
// phase_inc[0] = lv_cmake(cos(phase_step_rad), sin(phase_step_rad));
|
||||
// int n;
|
||||
// int num_a_vectors = 3;
|
||||
// float** in_a = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_a_vectors, volk_gnsssdr_get_alignment());
|
||||
// for (n = 0; n < num_a_vectors; n++)
|
||||
// {
|
||||
// in_a[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
|
||||
// memcpy((float*)in_a[n], (float*)in, sizeof(float) * num_points);
|
||||
// }
|
||||
// volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn_a_avx(result, local_code, phase_inc[0], phase, (const float**)in_a, num_a_vectors, num_points);
|
||||
//
|
||||
// for (n = 0; n < num_a_vectors; n++)
|
||||
// {
|
||||
// volk_gnsssdr_free(in_a[n]);
|
||||
// }
|
||||
// volk_gnsssdr_free(in_a);
|
||||
//}
|
||||
//
|
||||
//#endif // AVX
|
||||
|
||||
#endif // INCLUDED_volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc_H
|
||||
@ -99,6 +99,7 @@ std::vector<volk_gnsssdr_test_case_t> init_test_list(volk_gnsssdr_test_params_t
|
||||
QA(VOLK_INIT_PUPP(volk_gnsssdr_16ic_16i_rotator_dotprodxnpuppet_16ic, volk_gnsssdr_16ic_16i_rotator_dot_prod_16ic_xn, test_params_int16))
|
||||
QA(VOLK_INIT_PUPP(volk_gnsssdr_32fc_x2_rotator_dotprodxnpuppet_32fc, volk_gnsssdr_32fc_x2_rotator_dot_prod_32fc_xn, test_params_int1))
|
||||
QA(VOLK_INIT_PUPP(volk_gnsssdr_32fc_32f_rotator_dotprodxnpuppet_32fc, volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn, test_params_int1));
|
||||
QA(VOLK_INIT_PUPP(volk_gnsssdr_32fc_32f_high_dynamic_rotator_dotprodxnpuppet_32fc, volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn, test_params_int1));
|
||||
|
||||
return test_cases;
|
||||
}
|
||||
|
||||
@ -484,7 +484,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
|
||||
if (T_rx_clock_step_samples == 0)
|
||||
{
|
||||
T_rx_clock_step_samples = std::round(static_cast<double>(in[d_nchannels_in - 1][0].fs) * 1e-3); // 1 ms
|
||||
std::cout << "Observables clock step samples set to " << T_rx_clock_step_samples << std::endl;
|
||||
LOG(INFO) << "Observables clock step samples set to " << T_rx_clock_step_samples;
|
||||
usleep(1000000);
|
||||
}
|
||||
|
||||
|
||||
@ -59,6 +59,16 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking(
|
||||
trk_param.fs_in = fs_in;
|
||||
bool dump = configuration->property(role + ".dump", false);
|
||||
trk_param.dump = dump;
|
||||
trk_param.high_dyn = configuration->property(role + ".high_dyn", false);
|
||||
if (configuration->property(role + ".smoother_length", 10) < 1)
|
||||
{
|
||||
trk_param.smoother_length = 1;
|
||||
std::cout << TEXT_RED << "WARNING: Gal. E1. smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
trk_param.smoother_length = configuration->property(role + ".smoother_length", 10);
|
||||
}
|
||||
float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 5.0);
|
||||
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
|
||||
trk_param.pll_bw_hz = pll_bw_hz;
|
||||
|
||||
@ -59,6 +59,16 @@ GalileoE5aDllPllTracking::GalileoE5aDllPllTracking(
|
||||
trk_param.fs_in = fs_in;
|
||||
bool dump = configuration->property(role + ".dump", false);
|
||||
trk_param.dump = dump;
|
||||
trk_param.high_dyn = configuration->property(role + ".high_dyn", false);
|
||||
if (configuration->property(role + ".smoother_length", 10) < 1)
|
||||
{
|
||||
trk_param.smoother_length = 1;
|
||||
std::cout << TEXT_RED << "WARNING: Gal. E5a. smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
trk_param.smoother_length = configuration->property(role + ".smoother_length", 10);
|
||||
}
|
||||
float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 20.0);
|
||||
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
|
||||
trk_param.pll_bw_hz = pll_bw_hz;
|
||||
|
||||
@ -57,6 +57,16 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking(
|
||||
int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
|
||||
int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
|
||||
trk_param.fs_in = fs_in;
|
||||
trk_param.high_dyn = configuration->property(role + ".high_dyn", false);
|
||||
if (configuration->property(role + ".smoother_length", 10) < 1)
|
||||
{
|
||||
trk_param.smoother_length = 1;
|
||||
std::cout << TEXT_RED << "WARNING: GPS L1 C/A. smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
trk_param.smoother_length = configuration->property(role + ".smoother_length", 10);
|
||||
}
|
||||
bool dump = configuration->property(role + ".dump", false);
|
||||
trk_param.dump = dump;
|
||||
float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
|
||||
|
||||
@ -59,6 +59,16 @@ GpsL5DllPllTracking::GpsL5DllPllTracking(
|
||||
trk_param.fs_in = fs_in;
|
||||
bool dump = configuration->property(role + ".dump", false);
|
||||
trk_param.dump = dump;
|
||||
trk_param.high_dyn = configuration->property(role + ".high_dyn", false);
|
||||
if (configuration->property(role + ".smoother_length", 10) < 1)
|
||||
{
|
||||
trk_param.smoother_length = 1;
|
||||
std::cout << TEXT_RED << "WARNING: GPS L5. smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
trk_param.smoother_length = configuration->property(role + ".smoother_length", 10);
|
||||
}
|
||||
float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
|
||||
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
|
||||
trk_param.pll_bw_hz = pll_bw_hz;
|
||||
|
||||
@ -58,6 +58,7 @@
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <numeric>
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
@ -355,6 +356,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl
|
||||
{
|
||||
// Extra correlator for the data component
|
||||
correlator_data_cpu.init(2 * trk_parameters.vector_length, 1);
|
||||
correlator_data_cpu.set_high_dynamics_resampler(trk_parameters.high_dyn);
|
||||
d_data_code = static_cast<float *>(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment()));
|
||||
}
|
||||
else
|
||||
@ -363,7 +365,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl
|
||||
}
|
||||
|
||||
// --- Initializations ---
|
||||
multicorrelator_cpu.set_high_dynamics_resampler(trk_parameters.use_high_dynamics_resampler);
|
||||
multicorrelator_cpu.set_high_dynamics_resampler(trk_parameters.high_dyn);
|
||||
// Initial code frequency basis of NCO
|
||||
d_code_freq_chips = d_code_chip_rate;
|
||||
// Residual code phase (in chips)
|
||||
@ -397,12 +399,22 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl
|
||||
d_code_phase_step_chips = 0.0;
|
||||
d_code_phase_rate_step_chips = 0.0;
|
||||
d_carrier_phase_step_rad = 0.0;
|
||||
d_carrier_phase_rate_step_rad = 0.0;
|
||||
d_rem_code_phase_chips = 0.0;
|
||||
d_K_blk_samples = 0.0;
|
||||
d_code_phase_samples = 0.0;
|
||||
d_last_prompt = gr_complex(0.0, 0.0);
|
||||
d_state = 0; // initial state: standby
|
||||
clear_tracking_vars();
|
||||
if (trk_parameters.smoother_length > 0)
|
||||
{
|
||||
d_carr_ph_history.resize(trk_parameters.smoother_length * 2);
|
||||
d_code_ph_history.resize(trk_parameters.smoother_length * 2);
|
||||
}
|
||||
else
|
||||
{
|
||||
d_carr_ph_history.resize(1);
|
||||
d_code_ph_history.resize(1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -424,6 +436,7 @@ void dll_pll_veml_tracking::start_tracking()
|
||||
// new chip and PRN sequence periods based on acq Doppler
|
||||
d_code_freq_chips = radial_velocity * d_code_chip_rate;
|
||||
d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in;
|
||||
d_code_phase_rate_step_chips = 0.0;
|
||||
double T_chip_mod_seconds = 1.0 / d_code_freq_chips;
|
||||
double T_prn_mod_seconds = T_chip_mod_seconds * static_cast<double>(d_code_length_chips);
|
||||
double T_prn_mod_samples = T_prn_mod_seconds * trk_parameters.fs_in;
|
||||
@ -446,7 +459,9 @@ void dll_pll_veml_tracking::start_tracking()
|
||||
|
||||
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
|
||||
d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters.fs_in;
|
||||
|
||||
d_carrier_phase_rate_step_rad = 0.0;
|
||||
d_carr_ph_history.clear();
|
||||
d_code_ph_history.clear();
|
||||
// DLL/PLL filter initialization
|
||||
d_carrier_loop_filter.initialize(); // initialize the carrier filter
|
||||
d_code_loop_filter.initialize(); // initialize the code filter
|
||||
@ -706,7 +721,7 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples)
|
||||
multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, input_samples);
|
||||
multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(
|
||||
d_rem_carr_phase_rad,
|
||||
d_carrier_phase_step_rad,
|
||||
d_carrier_phase_step_rad, d_carrier_phase_rate_step_rad,
|
||||
static_cast<float>(d_rem_code_phase_chips) * static_cast<float>(d_code_samples_per_chip),
|
||||
static_cast<float>(d_code_phase_step_chips) * static_cast<float>(d_code_samples_per_chip),
|
||||
static_cast<float>(d_code_phase_rate_step_chips) * static_cast<float>(d_code_samples_per_chip),
|
||||
@ -718,7 +733,7 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples)
|
||||
correlator_data_cpu.set_input_output_vectors(d_Prompt_Data, input_samples);
|
||||
correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler(
|
||||
d_rem_carr_phase_rad,
|
||||
d_carrier_phase_step_rad,
|
||||
d_carrier_phase_step_rad, d_carrier_phase_rate_step_rad,
|
||||
static_cast<float>(d_rem_code_phase_chips) * static_cast<float>(d_code_samples_per_chip),
|
||||
static_cast<float>(d_code_phase_step_chips) * static_cast<float>(d_code_samples_per_chip),
|
||||
static_cast<float>(d_code_phase_rate_step_chips) * static_cast<float>(d_code_samples_per_chip),
|
||||
@ -777,6 +792,10 @@ void dll_pll_veml_tracking::clear_tracking_vars()
|
||||
d_current_symbol = 0;
|
||||
d_Prompt_buffer_deque.clear();
|
||||
d_last_prompt = gr_complex(0.0, 0.0);
|
||||
d_carrier_phase_rate_step_rad = 0.0;
|
||||
d_code_phase_rate_step_chips = 0.0;
|
||||
d_carr_ph_history.clear();
|
||||
d_code_ph_history.clear();
|
||||
}
|
||||
|
||||
|
||||
@ -796,15 +815,60 @@ void dll_pll_veml_tracking::update_tracking_vars()
|
||||
//################### PLL COMMANDS #################################################
|
||||
// carrier phase step (NCO phase increment per sample) [rads/sample]
|
||||
d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters.fs_in;
|
||||
// carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2]
|
||||
if (trk_parameters.high_dyn)
|
||||
{
|
||||
d_carr_ph_history.push_back(std::pair<double, double>(d_carrier_phase_step_rad, static_cast<double>(d_current_prn_length_samples)));
|
||||
if (d_carr_ph_history.full())
|
||||
{
|
||||
double tmp_cp1 = 0.0;
|
||||
double tmp_cp2 = 0.0;
|
||||
double tmp_samples = 0.0;
|
||||
for (unsigned int k = 0; k < trk_parameters.smoother_length; k++)
|
||||
{
|
||||
tmp_cp1 += d_carr_ph_history.at(k).first;
|
||||
tmp_cp2 += d_carr_ph_history.at(trk_parameters.smoother_length * 2 - k - 1).first;
|
||||
tmp_samples += d_carr_ph_history.at(trk_parameters.smoother_length * 2 - k - 1).second;
|
||||
}
|
||||
tmp_cp1 /= static_cast<double>(trk_parameters.smoother_length);
|
||||
tmp_cp2 /= static_cast<double>(trk_parameters.smoother_length);
|
||||
d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples;
|
||||
}
|
||||
}
|
||||
//std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl;
|
||||
// remnant carrier phase to prevent overflow in the code NCO
|
||||
d_rem_carr_phase_rad += d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples);
|
||||
d_rem_carr_phase_rad += static_cast<float>(d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples));
|
||||
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2);
|
||||
|
||||
|
||||
// carrier phase accumulator
|
||||
d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples);
|
||||
//double a = d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples);
|
||||
//double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples);
|
||||
//std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl;
|
||||
d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples));
|
||||
|
||||
//################### DLL COMMANDS #################################################
|
||||
// code phase step (Code resampler phase increment per sample) [chips/sample]
|
||||
d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in;
|
||||
if (trk_parameters.high_dyn)
|
||||
{
|
||||
d_code_ph_history.push_back(std::pair<double, double>(d_code_phase_step_chips, static_cast<double>(d_current_prn_length_samples)));
|
||||
if (d_code_ph_history.full())
|
||||
{
|
||||
double tmp_cp1 = 0.0;
|
||||
double tmp_cp2 = 0.0;
|
||||
double tmp_samples = 0.0;
|
||||
for (unsigned int k = 0; k < trk_parameters.smoother_length; k++)
|
||||
{
|
||||
tmp_cp1 += d_code_ph_history.at(k).first;
|
||||
tmp_cp2 += d_code_ph_history.at(trk_parameters.smoother_length * 2 - k - 1).first;
|
||||
tmp_samples += d_code_ph_history.at(trk_parameters.smoother_length * 2 - k - 1).second;
|
||||
}
|
||||
tmp_cp1 /= static_cast<double>(trk_parameters.smoother_length);
|
||||
tmp_cp2 /= static_cast<double>(trk_parameters.smoother_length);
|
||||
d_code_phase_rate_step_chips = (tmp_cp2 - tmp_cp1) / tmp_samples;
|
||||
}
|
||||
}
|
||||
// remnant code phase [chips]
|
||||
d_rem_code_phase_samples = K_blk_samples - static_cast<double>(d_current_prn_length_samples); // rounding error < 1 sample
|
||||
d_rem_code_phase_chips = d_code_freq_chips * d_rem_code_phase_samples / trk_parameters.fs_in;
|
||||
@ -947,8 +1011,14 @@ void dll_pll_veml_tracking::log_data(bool integrating)
|
||||
// carrier and code frequency
|
||||
tmp_float = d_carrier_doppler_hz;
|
||||
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
|
||||
// carrier phase rate [Hz/s]
|
||||
tmp_float = d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2;
|
||||
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
|
||||
tmp_float = d_code_freq_chips;
|
||||
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
|
||||
// code phase rate [chips/s^2]
|
||||
tmp_float = d_code_phase_rate_step_chips * trk_parameters.fs_in * trk_parameters.fs_in;
|
||||
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
|
||||
// PLL commands
|
||||
tmp_float = d_carr_error_hz;
|
||||
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
|
||||
@ -986,7 +1056,7 @@ int32_t dll_pll_veml_tracking::save_matfile()
|
||||
// READ DUMP FILE
|
||||
std::ifstream::pos_type size;
|
||||
int32_t number_of_double_vars = 1;
|
||||
int32_t number_of_float_vars = 17;
|
||||
int32_t number_of_float_vars = 19;
|
||||
int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
|
||||
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
|
||||
std::ifstream dump_file;
|
||||
@ -1022,7 +1092,9 @@ int32_t dll_pll_veml_tracking::save_matfile()
|
||||
uint64_t *PRN_start_sample_count = new uint64_t[num_epoch];
|
||||
float *acc_carrier_phase_rad = new float[num_epoch];
|
||||
float *carrier_doppler_hz = new float[num_epoch];
|
||||
float *carrier_doppler_rate_hz = new float[num_epoch];
|
||||
float *code_freq_chips = new float[num_epoch];
|
||||
float *code_freq_rate_chips = new float[num_epoch];
|
||||
float *carr_error_hz = new float[num_epoch];
|
||||
float *carr_error_filt_hz = new float[num_epoch];
|
||||
float *code_error_chips = new float[num_epoch];
|
||||
@ -1049,7 +1121,9 @@ int32_t dll_pll_veml_tracking::save_matfile()
|
||||
dump_file.read(reinterpret_cast<char *>(&PRN_start_sample_count[i]), sizeof(uint64_t));
|
||||
dump_file.read(reinterpret_cast<char *>(&acc_carrier_phase_rad[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&carrier_doppler_hz[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&carrier_doppler_rate_hz[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&code_freq_chips[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&code_freq_rate_chips[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&carr_error_hz[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&carr_error_filt_hz[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&code_error_chips[i]), sizeof(float));
|
||||
@ -1076,7 +1150,9 @@ int32_t dll_pll_veml_tracking::save_matfile()
|
||||
delete[] PRN_start_sample_count;
|
||||
delete[] acc_carrier_phase_rad;
|
||||
delete[] carrier_doppler_hz;
|
||||
delete[] carrier_doppler_rate_hz;
|
||||
delete[] code_freq_chips;
|
||||
delete[] code_freq_rate_chips;
|
||||
delete[] carr_error_hz;
|
||||
delete[] carr_error_filt_hz;
|
||||
delete[] code_error_chips;
|
||||
@ -1139,10 +1215,18 @@ int32_t dll_pll_veml_tracking::save_matfile()
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
@ -1190,7 +1274,9 @@ int32_t dll_pll_veml_tracking::save_matfile()
|
||||
delete[] PRN_start_sample_count;
|
||||
delete[] acc_carrier_phase_rad;
|
||||
delete[] carrier_doppler_hz;
|
||||
delete[] carrier_doppler_rate_hz;
|
||||
delete[] code_freq_chips;
|
||||
delete[] code_freq_rate_chips;
|
||||
delete[] carr_error_hz;
|
||||
delete[] carr_error_filt_hz;
|
||||
delete[] code_error_chips;
|
||||
|
||||
@ -41,6 +41,7 @@
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <queue>
|
||||
#include <utility>
|
||||
#include <boost/circular_buffer.hpp>
|
||||
|
||||
class dll_pll_veml_tracking;
|
||||
@ -146,10 +147,13 @@ private:
|
||||
|
||||
double d_code_phase_step_chips;
|
||||
double d_code_phase_rate_step_chips;
|
||||
boost::circular_buffer<std::pair<double, double>> d_code_ph_history;
|
||||
double d_carrier_phase_step_rad;
|
||||
double d_carrier_phase_rate_step_rad;
|
||||
boost::circular_buffer<std::pair<double, double>> d_carr_ph_history;
|
||||
// remaining code phase and carrier phase between tracking loops
|
||||
double d_rem_code_phase_samples;
|
||||
double d_rem_carr_phase_rad;
|
||||
float d_rem_carr_phase_rad;
|
||||
|
||||
// PLL and DLL filter library
|
||||
Tracking_2nd_DLL_filter d_code_loop_filter;
|
||||
@ -164,7 +168,6 @@ private:
|
||||
double d_carr_error_filt_hz;
|
||||
double d_code_error_chips;
|
||||
double d_code_error_filt_chips;
|
||||
double d_K_blk_samples;
|
||||
double d_code_freq_chips;
|
||||
double d_carrier_doppler_hz;
|
||||
double d_acc_carrier_phase_rad;
|
||||
|
||||
@ -125,7 +125,32 @@ void cpu_multicorrelator_real_codes::update_local_code(int correlator_length_sam
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Overload Carrier_wipeoff_multicorrelator_resampler to ensure back compatibility
|
||||
bool cpu_multicorrelator_real_codes::Carrier_wipeoff_multicorrelator_resampler(
|
||||
float rem_carrier_phase_in_rad,
|
||||
float phase_step_rad,
|
||||
float phase_rate_step_rad,
|
||||
float rem_code_phase_chips,
|
||||
float code_phase_step_chips,
|
||||
float code_phase_rate_step_chips,
|
||||
int signal_length_samples)
|
||||
{
|
||||
update_local_code(signal_length_samples, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips);
|
||||
// Regenerate phase at each call in order to avoid numerical issues
|
||||
lv_32fc_t phase_offset_as_complex[1];
|
||||
phase_offset_as_complex[0] = lv_cmake(std::cos(rem_carrier_phase_in_rad), -std::sin(rem_carrier_phase_in_rad));
|
||||
// call VOLK_GNSSSDR kernel
|
||||
if (d_use_high_dynamics_resampler)
|
||||
{
|
||||
volk_gnsssdr_32fc_32f_high_dynamic_rotator_dot_prod_32fc_xn(d_corr_out, d_sig_in, std::exp(lv_32fc_t(0.0, -phase_step_rad)), std::exp(lv_32fc_t(0.0, -phase_rate_step_rad)), phase_offset_as_complex, const_cast<const float**>(d_local_codes_resampled), d_n_correlators, signal_length_samples);
|
||||
}
|
||||
else
|
||||
{
|
||||
volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn(d_corr_out, d_sig_in, std::exp(lv_32fc_t(0.0, -phase_step_rad)), phase_offset_as_complex, const_cast<const float**>(d_local_codes_resampled), d_n_correlators, signal_length_samples);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
// Overload Carrier_wipeoff_multicorrelator_resampler to ensure back compatibility
|
||||
bool cpu_multicorrelator_real_codes::Carrier_wipeoff_multicorrelator_resampler(
|
||||
float rem_carrier_phase_in_rad,
|
||||
float phase_step_rad,
|
||||
|
||||
@ -52,6 +52,8 @@ public:
|
||||
bool set_local_code_and_taps(int code_length_chips, const float *local_code_in, float *shifts_chips);
|
||||
bool set_input_output_vectors(std::complex<float> *corr_out, const std::complex<float> *sig_in);
|
||||
void update_local_code(int correlator_length_samples, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips = 0.0);
|
||||
// Overload Carrier_wipeoff_multicorrelator_resampler to ensure back compatibility
|
||||
bool Carrier_wipeoff_multicorrelator_resampler(float rem_carrier_phase_in_rad, float phase_step_rad, float phase_rate_step_rad, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, int signal_length_samples);
|
||||
bool Carrier_wipeoff_multicorrelator_resampler(float rem_carrier_phase_in_rad, float phase_step_rad, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, int signal_length_samples);
|
||||
bool free();
|
||||
|
||||
|
||||
@ -36,7 +36,8 @@
|
||||
Dll_Pll_Conf::Dll_Pll_Conf()
|
||||
{
|
||||
/* DLL/PLL tracking configuration */
|
||||
use_high_dynamics_resampler = true;
|
||||
high_dyn = false;
|
||||
smoother_length = 10;
|
||||
fs_in = 0.0;
|
||||
vector_length = 0U;
|
||||
dump = false;
|
||||
|
||||
@ -56,11 +56,12 @@ public:
|
||||
float early_late_space_narrow_chips;
|
||||
float very_early_late_space_narrow_chips;
|
||||
int32_t extend_correlation_symbols;
|
||||
bool use_high_dynamics_resampler;
|
||||
bool high_dyn;
|
||||
int32_t cn0_samples;
|
||||
int32_t carrier_lock_det_mav_samples;
|
||||
int32_t cn0_min;
|
||||
int32_t max_lock_fail;
|
||||
uint32_t smoother_length;
|
||||
double carrier_lock_th;
|
||||
bool track_pilot;
|
||||
char system;
|
||||
|
||||
@ -228,7 +228,10 @@ if(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA)
|
||||
find_package(GPSTK)
|
||||
if(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK)
|
||||
message(STATUS "GPSTk v${GNSSSDR_GPSTK_LOCAL_VERSION} will be automatically downloaded and built when doing 'make'.")
|
||||
|
||||
if ("${TOOLCHAIN_ARG}" STREQUAL "")
|
||||
set(TOOLCHAIN_ARG "-DCMAKE_CXX_FLAGS=\"-Wno-deprecated\"")
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated")
|
||||
endif("${TOOLCHAIN_ARG}" STREQUAL "")
|
||||
# if(NOT ENABLE_FPGA)
|
||||
if(CMAKE_VERSION VERSION_LESS 3.2)
|
||||
ExternalProject_Add(
|
||||
@ -403,6 +406,7 @@ if(ENABLE_UNIT_TESTING)
|
||||
signal_generator_adapters
|
||||
pvt_gr_blocks
|
||||
signal_processing_testing_lib
|
||||
system_testing_lib
|
||||
${VOLK_GNSSSDR_LIBRARIES}
|
||||
${MATIO_LIBRARIES}
|
||||
${GNSS_SDR_TEST_OPTIONAL_LIBS}
|
||||
|
||||
@ -41,7 +41,7 @@ DEFINE_bool(enable_external_signal_file, false, "Use an external signal file cap
|
||||
DEFINE_double(external_signal_acquisition_threshold, 2.5, "Threshold for satellite acquisition when external file is used");
|
||||
DEFINE_int32(external_signal_acquisition_dwells, 5, "Maximum dwells count for satellite acquisition when external file is used");
|
||||
DEFINE_double(external_signal_acquisition_doppler_max_hz, 5000.0, "Doppler max for satellite acquisition when external file is used");
|
||||
DEFINE_double(external_signal_acquisition_doppler_step_hz, 125, "Doppler step for satellite acquisition when external file is used");
|
||||
DEFINE_double(external_signal_acquisition_doppler_step_hz, 125.0, "Doppler step for satellite acquisition when external file is used");
|
||||
|
||||
DEFINE_string(signal_file, std::string("signal_out.bin"), "Path of the external signal capture file");
|
||||
DEFINE_double(CN0_dBHz_start, std::numeric_limits<double>::infinity(), "Enable noise generator and set the CN0 start sweep value [dB-Hz]");
|
||||
@ -77,6 +77,8 @@ DEFINE_double(skip_trk_transitory_s, 1.0, "Skip the initial tracking output sign
|
||||
|
||||
//Tracking configuration
|
||||
DEFINE_int32(extend_correlation_symbols, 1, "Set the tracking coherent correlation to N symbols (up to 20 for GPS L1 C/A)");
|
||||
DEFINE_int32(smoother_length, 10, "Set the moving average size for the carrier phase and code phase in case of high dynamics");
|
||||
DEFINE_bool(high_dyn, false, "Activates the code resampler and NCO generator for high dynamics");
|
||||
|
||||
//Test output configuration
|
||||
DEFINE_bool(plot_gps_l1_tracking_test, false, "Plots results of GpsL1CADllPllTrackingTest with gnuplot");
|
||||
|
||||
@ -28,10 +28,10 @@
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "geofunctions.h"
|
||||
|
||||
const double STRP_G_SI = 9.80665;
|
||||
const double STRP_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E
|
||||
const double STRP_PI = 3.1415926535898; // Pi as defined in IS-GPS-200E
|
||||
|
||||
arma::mat Skew_symmetric(const arma::vec &a)
|
||||
{
|
||||
@ -205,17 +205,17 @@ int togeod(double *dphi, double *dlambda, double *h, double a, double finv, doub
|
||||
cosphi = cos(*dphi);
|
||||
|
||||
// compute radius of curvature in prime vertical direction
|
||||
N_phi = a / sqrt(1 - esq * sinphi * sinphi);
|
||||
N_phi = a / sqrt(1.0 - esq * sinphi * sinphi);
|
||||
|
||||
// compute residuals in P and Z
|
||||
// compute residuals in P and Z
|
||||
dP = P - (N_phi + (*h)) * cosphi;
|
||||
dZ = Z - (N_phi * oneesq + (*h)) * sinphi;
|
||||
|
||||
// update height and latitude
|
||||
// update height and latitude
|
||||
*h = *h + (sinphi * dZ + cosphi * dP);
|
||||
*dphi = *dphi + (cosphi * dZ - sinphi * dP) / (N_phi + (*h));
|
||||
|
||||
// test for convergence
|
||||
// test for convergence
|
||||
if ((dP * dP + dZ * dZ) < tolsq)
|
||||
{
|
||||
break;
|
||||
@ -233,7 +233,7 @@ int togeod(double *dphi, double *dlambda, double *h, double a, double finv, doub
|
||||
arma::mat Gravity_ECEF(const arma::vec &r_eb_e)
|
||||
{
|
||||
// Parameters
|
||||
const double R_0 = 6378137; // WGS84 Equatorial radius in meters
|
||||
const double R_0 = 6378137.0; // WGS84 Equatorial radius in meters
|
||||
const double mu = 3.986004418E14; // WGS84 Earth gravitational constant (m^3 s^-2)
|
||||
const double J_2 = 1.082627E-3; // WGS84 Earth's second gravitational constant
|
||||
const double omega_ie = 7.292115E-5; // Earth rotation rate (rad/s)
|
||||
@ -259,12 +259,14 @@ arma::mat Gravity_ECEF(const arma::vec &r_eb_e)
|
||||
}
|
||||
|
||||
|
||||
arma::vec LLH_to_deg(arma::vec &LLH)
|
||||
arma::vec LLH_to_deg(const arma::vec &LLH)
|
||||
{
|
||||
const double rtd = 180.0 / STRP_PI;
|
||||
LLH(0) = LLH(0) * rtd;
|
||||
LLH(1) = LLH(1) * rtd;
|
||||
return LLH;
|
||||
arma::vec deg = arma::zeros(3, 1);
|
||||
deg(0) = LLH(0) * rtd;
|
||||
deg(1) = LLH(1) * rtd;
|
||||
deg(2) = LLH(2);
|
||||
return deg;
|
||||
}
|
||||
|
||||
|
||||
@ -296,15 +298,16 @@ double mstokph(double MetersPerSeconds)
|
||||
}
|
||||
|
||||
|
||||
arma::vec CTM_to_Euler(arma::mat &C)
|
||||
arma::vec CTM_to_Euler(const arma::mat &C)
|
||||
{
|
||||
// Calculate Euler angles using (2.23)
|
||||
arma::mat CTM = C;
|
||||
arma::vec eul = arma::zeros(3, 1);
|
||||
eul(0) = atan2(C(1, 2), C(2, 2)); // roll
|
||||
if (C(0, 2) < -1.0) C(0, 2) = -1.0;
|
||||
if (C(0, 2) > 1.0) C(0, 2) = 1.0;
|
||||
eul(1) = -asin(C(0, 2)); // pitch
|
||||
eul(2) = atan2(C(0, 1), C(0, 0)); // yaw
|
||||
eul(0) = atan2(CTM(1, 2), CTM(2, 2)); // roll
|
||||
if (CTM(0, 2) < -1.0) CTM(0, 2) = -1.0;
|
||||
if (CTM(0, 2) > 1.0) CTM(0, 2) = 1.0;
|
||||
eul(1) = -asin(CTM(0, 2)); // pitch
|
||||
eul(2) = atan2(CTM(0, 1), CTM(0, 0)); // yaw
|
||||
return eul;
|
||||
}
|
||||
|
||||
@ -353,19 +356,19 @@ arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection)
|
||||
do
|
||||
{
|
||||
oldh = h;
|
||||
N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi)));
|
||||
N = c / sqrt(1.0 + ex2 * (cos(phi) * cos(phi)));
|
||||
phi = atan(XYZ[2] / ((sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h)))));
|
||||
h = sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) / cos(phi) - N;
|
||||
iterations = iterations + 1;
|
||||
if (iterations > 100)
|
||||
{
|
||||
//std::cout << "Failed to approximate h with desired precision. h-oldh= " << h - oldh;
|
||||
// std::cout << "Failed to approximate h with desired precision. h-oldh= " << h - oldh;
|
||||
break;
|
||||
}
|
||||
}
|
||||
while (std::abs(h - oldh) > 1.0e-12);
|
||||
while (std::fabs(h - oldh) > 1.0e-12);
|
||||
|
||||
arma::vec LLH = {{phi, lambda, h}}; //radians
|
||||
arma::vec LLH = {{phi, lambda, h}}; // radians
|
||||
return LLH;
|
||||
}
|
||||
|
||||
@ -398,11 +401,11 @@ void ECEF_to_Geo(const arma::vec &r_eb_e, const arma::vec &v_eb_e, const arma::m
|
||||
void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat &C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e)
|
||||
{
|
||||
// Parameters
|
||||
double R_0 = 6378137; // WGS84 Equatorial radius in meters
|
||||
double R_0 = 6378137.0; // WGS84 Equatorial radius in meters
|
||||
double e = 0.0818191908425; // WGS84 eccentricity
|
||||
|
||||
// Calculate transverse radius of curvature using (2.105)
|
||||
double R_E = R_0 / sqrt(1 - (e * sin(LLH(0))) * (e * sin(LLH(0))));
|
||||
double R_E = R_0 / sqrt(1.0 - (e * sin(LLH(0))) * (e * sin(LLH(0))));
|
||||
|
||||
// Convert position using (2.112)
|
||||
double cos_lat = cos(LLH(0));
|
||||
@ -434,7 +437,7 @@ void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat
|
||||
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e)
|
||||
{
|
||||
// Parameters
|
||||
const double R_0 = 6378137; // WGS84 Equatorial radius in meters
|
||||
const double R_0 = 6378137.0; // WGS84 Equatorial radius in meters
|
||||
const double e = 0.0818191908425; // WGS84 eccentricity
|
||||
|
||||
// Calculate transverse radius of curvature using (2.105)
|
||||
@ -458,3 +461,315 @@ void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_
|
||||
// Transform velocity using (2.73)
|
||||
v_eb_e = C_e_n.t() * v_eb_n;
|
||||
}
|
||||
|
||||
|
||||
double great_circle_distance(double lat1, double lon1, double lat2, double lon2)
|
||||
{
|
||||
// The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes.
|
||||
// generally used geo measurement function
|
||||
double R = 6378.137; // Radius of earth in KM
|
||||
double dLat = lat2 * STRP_PI / 180.0 - lat1 * STRP_PI / 180.0;
|
||||
double dLon = lon2 * STRP_PI / 180.0 - lon1 * STRP_PI / 180.0;
|
||||
double a = sin(dLat / 2.0) * sin(dLat / 2.0) +
|
||||
cos(lat1 * STRP_PI / 180.0) * cos(lat2 * STRP_PI / 180.0) *
|
||||
sin(dLon / 2) * sin(dLon / 2.0);
|
||||
double c = 2.0 * atan2(sqrt(a), sqrt(1.0 - a));
|
||||
double d = R * c;
|
||||
return d * 1000.0; // meters
|
||||
}
|
||||
|
||||
|
||||
void cart2utm(const arma::vec &r_eb_e, int zone, arma::vec &r_enu)
|
||||
{
|
||||
// Transformation of (X,Y,Z) to (E,N,U) in UTM, zone 'zone'
|
||||
//
|
||||
// Inputs:
|
||||
// r_eb_e - Cartesian coordinates. Coordinates are referenced
|
||||
// with respect to the International Terrestrial Reference
|
||||
// Frame 1996 (ITRF96)
|
||||
// zone - UTM zone of the given position
|
||||
//
|
||||
// Outputs:
|
||||
// r_enu - UTM coordinates (Easting, Northing, Uping)
|
||||
//
|
||||
// Originally written in Matlab by Kai Borre, Nov. 1994
|
||||
// Implemented in C++ by J.Arribas
|
||||
//
|
||||
// This implementation is based upon
|
||||
// O. Andersson & K. Poder (1981) Koordinattransformationer
|
||||
// ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren
|
||||
// Vol. 30: 552--571 and Vol. 31: 76
|
||||
//
|
||||
// An excellent, general reference (KW) is
|
||||
// R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der
|
||||
// h\"oheren Geod\"asie und Kartographie.
|
||||
// Erster Band, Springer Verlag
|
||||
//
|
||||
// Explanation of variables used:
|
||||
// f flattening of ellipsoid
|
||||
// a semi major axis in m
|
||||
// m0 1 - scale at central meridian; for UTM 0.0004
|
||||
// Q_n normalized meridian quadrant
|
||||
// E0 Easting of central meridian
|
||||
// L0 Longitude of central meridian
|
||||
// bg constants for ellipsoidal geogr. to spherical geogr.
|
||||
// gb constants for spherical geogr. to ellipsoidal geogr.
|
||||
// gtu constants for ellipsoidal N, E to spherical N, E
|
||||
// utg constants for spherical N, E to ellipoidal N, E
|
||||
// tolutm tolerance for utm, 1.2E-10*meridian quadrant
|
||||
// tolgeo tolerance for geographical, 0.00040 second of arc
|
||||
//
|
||||
// B, L refer to latitude and longitude. Southern latitude is negative
|
||||
// International ellipsoid of 1924, valid for ED50
|
||||
|
||||
double a = 6378388.0;
|
||||
double f = 1.0 / 297.0;
|
||||
double ex2 = (2.0 - f) * f / ((1.0 - f) * (1.0 - f));
|
||||
double c = a * sqrt(1.0 + ex2);
|
||||
arma::vec vec = r_eb_e;
|
||||
vec(2) = vec(2) - 4.5;
|
||||
double alpha = 0.756e-6;
|
||||
arma::mat R = {{1.0, -alpha, 0.0}, {alpha, 1.0, 0.0}, {0.0, 0.0, 1.0}};
|
||||
arma::vec trans = {89.5, 93.8, 127.6};
|
||||
double scale = 0.9999988;
|
||||
arma::vec v = scale * R * vec + trans; // coordinate vector in ED50
|
||||
double L = atan2(v(1), v(0));
|
||||
double N1 = 6395000.0; // preliminary value
|
||||
double B = atan2(v(2) / ((1.0 - f) * (1.0 - f) * N1), arma::norm(v.subvec(0, 1)) / N1); // preliminary value
|
||||
double U = 0.1;
|
||||
double oldU = 0.0;
|
||||
int iterations = 0;
|
||||
while (fabs(U - oldU) > 1.0E-4)
|
||||
{
|
||||
oldU = U;
|
||||
N1 = c / sqrt(1.0 + ex2 * (cos(B) * cos(B)));
|
||||
B = atan2(v(2) / ((1.0 - f) * (1.0 - f) * N1 + U), arma::norm(v.subvec(0, 1)) / (N1 + U));
|
||||
U = arma::norm(v.subvec(0, 1)) / cos(B) - N1;
|
||||
iterations = iterations + 1;
|
||||
if (iterations > 100)
|
||||
{
|
||||
std::cout << "Failed to approximate U with desired precision. U-oldU:" << U - oldU << std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
// Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21)
|
||||
double m0 = 0.0004;
|
||||
double n = f / (2.0 - f);
|
||||
double m = n * n * (1.0 / 4.0 + n * n / 64.0);
|
||||
double w = (a * (-n - m0 + m * (1.0 - m0))) / (1.0 + n);
|
||||
double Q_n = a + w;
|
||||
|
||||
// Easting and longitude of central meridian
|
||||
double E0 = 500000.0;
|
||||
double L0 = (zone - 30) * 6.0 - 3.0;
|
||||
|
||||
// Check tolerance for reverse transformation
|
||||
// double tolutm = STRP_PI / 2.0 * 1.2e-10 * Q_n;
|
||||
// double tolgeo = 0.000040;
|
||||
// Coefficients of trigonometric series
|
||||
//
|
||||
// ellipsoidal to spherical geographical, KW p .186 --187, (51) - (52)
|
||||
// bg[1] = n * (-2 + n * (2 / 3 + n * (4 / 3 + n * (-82 / 45))));
|
||||
// bg[2] = n ^ 2 * (5 / 3 + n * (-16 / 15 + n * (-13 / 9)));
|
||||
// bg[3] = n ^ 3 * (-26 / 15 + n * 34 / 21);
|
||||
// bg[4] = n ^ 4 * 1237 / 630;
|
||||
//
|
||||
// spherical to ellipsoidal geographical, KW p.190 --191, (61) - (62) % gb[1] = n * (2 + n * (-2 / 3 + n * (-2 + n * 116 / 45)));
|
||||
// gb[2] = n ^ 2 * (7 / 3 + n * (-8 / 5 + n * (-227 / 45)));
|
||||
// gb[3] = n ^ 3 * (56 / 15 + n * (-136 / 35));
|
||||
// gb[4] = n ^ 4 * 4279 / 630;
|
||||
//
|
||||
// spherical to ellipsoidal N, E, KW p.196, (69) % gtu[1] = n * (1 / 2 + n * (-2 / 3 + n * (5 / 16 + n * 41 / 180)));
|
||||
// gtu[2] = n ^ 2 * (13 / 48 + n * (-3 / 5 + n * 557 / 1440));
|
||||
// gtu[3] = n ^ 3 * (61 / 240 + n * (-103 / 140));
|
||||
// gtu[4] = n ^ 4 * 49561 / 161280;
|
||||
//
|
||||
// ellipsoidal to spherical N, E, KW p.194, (65) % utg[1] = n * (-1 / 2 + n * (2 / 3 + n * (-37 / 96 + n * 1 / 360)));
|
||||
// utg[2] = n ^ 2 * (-1 / 48 + n * (-1 / 15 + n * 437 / 1440));
|
||||
// utg[3] = n ^ 3 * (-17 / 480 + n * 37 / 840);
|
||||
// utg[4] = n ^ 4 * (-4397 / 161280);
|
||||
//
|
||||
// With f = 1 / 297 we get
|
||||
|
||||
arma::colvec bg = {-3.37077907e-3,
|
||||
4.73444769e-6,
|
||||
-8.29914570e-9,
|
||||
1.58785330e-11};
|
||||
|
||||
arma::colvec gb = {3.37077588e-3,
|
||||
6.62769080e-6,
|
||||
1.78718601e-8,
|
||||
5.49266312e-11};
|
||||
|
||||
arma::colvec gtu = {8.41275991e-4,
|
||||
7.67306686e-7,
|
||||
1.21291230e-9,
|
||||
2.48508228e-12};
|
||||
|
||||
arma::colvec utg = {-8.41276339e-4,
|
||||
-5.95619298e-8,
|
||||
-1.69485209e-10,
|
||||
-2.20473896e-13};
|
||||
|
||||
// Ellipsoidal latitude, longitude to spherical latitude, longitude
|
||||
bool neg_geo = false;
|
||||
|
||||
if (B < 0.0) neg_geo = true;
|
||||
|
||||
double Bg_r = fabs(B);
|
||||
double res_clensin = clsin(bg, 4, 2.0 * Bg_r);
|
||||
Bg_r = Bg_r + res_clensin;
|
||||
L0 = L0 * STRP_PI / 180.0;
|
||||
double Lg_r = L - L0;
|
||||
|
||||
// Spherical latitude, longitude to complementary spherical latitude % i.e.spherical N, E
|
||||
double cos_BN = cos(Bg_r);
|
||||
double Np = atan2(sin(Bg_r), cos(Lg_r) * cos_BN);
|
||||
double Ep = atanh(sin(Lg_r) * cos_BN);
|
||||
|
||||
// Spherical normalized N, E to ellipsoidal N, E
|
||||
Np = 2.0 * Np;
|
||||
Ep = 2.0 * Ep;
|
||||
|
||||
double dN;
|
||||
double dE;
|
||||
clksin(gtu, 4, Np, Ep, &dN, &dE);
|
||||
Np = Np / 2.0;
|
||||
Ep = Ep / 2.0;
|
||||
Np = Np + dN;
|
||||
Ep = Ep + dE;
|
||||
double N = Q_n * Np;
|
||||
double E = Q_n * Ep + E0;
|
||||
if (neg_geo)
|
||||
{
|
||||
N = -N + 20000000.0;
|
||||
}
|
||||
r_enu(0) = E;
|
||||
r_enu(1) = N;
|
||||
r_enu(2) = U;
|
||||
}
|
||||
|
||||
|
||||
double clsin(const arma::colvec &ar, int degree, double argument)
|
||||
{
|
||||
// Clenshaw summation of sinus of argument.
|
||||
//
|
||||
// result = clsin(ar, degree, argument);
|
||||
//
|
||||
// Originally written in Matlab by Kai Borre
|
||||
// Implemented in C++ by J.Arribas
|
||||
|
||||
double cos_arg = 2.0 * cos(argument);
|
||||
double hr1 = 0.0;
|
||||
double hr = 0.0;
|
||||
double hr2;
|
||||
for (int t = degree; t > 0; t--)
|
||||
{
|
||||
hr2 = hr1;
|
||||
hr1 = hr;
|
||||
hr = ar(t - 1) + cos_arg * hr1 - hr2;
|
||||
}
|
||||
|
||||
return (hr * sin(argument));
|
||||
}
|
||||
|
||||
|
||||
void clksin(const arma::colvec &ar, int degree, double arg_real, double arg_imag, double *re, double *im)
|
||||
{
|
||||
// Clenshaw summation of sinus with complex argument
|
||||
// [re, im] = clksin(ar, degree, arg_real, arg_imag);
|
||||
//
|
||||
// Originally written in Matlab by Kai Borre
|
||||
// Implemented in C++ by J.Arribas
|
||||
|
||||
double sin_arg_r = sin(arg_real);
|
||||
double cos_arg_r = cos(arg_real);
|
||||
double sinh_arg_i = sinh(arg_imag);
|
||||
double cosh_arg_i = cosh(arg_imag);
|
||||
|
||||
double r = 2.0 * cos_arg_r * cosh_arg_i;
|
||||
double i = -2.0 * sin_arg_r * sinh_arg_i;
|
||||
|
||||
double hr1 = 0.0;
|
||||
double hr = 0.0;
|
||||
double hi1 = 0.0;
|
||||
double hi = 0.0;
|
||||
double hi2;
|
||||
double hr2;
|
||||
for (int t = degree; t > 0; t--)
|
||||
{
|
||||
hr2 = hr1;
|
||||
hr1 = hr;
|
||||
hi2 = hi1;
|
||||
hi1 = hi;
|
||||
double z = ar(t - 1) + r * hr1 - i * hi - hr2;
|
||||
hi = i * hr1 + r * hi1 - hi2;
|
||||
hr = z;
|
||||
}
|
||||
|
||||
r = sin_arg_r * cosh_arg_i;
|
||||
i = cos_arg_r * sinh_arg_i;
|
||||
|
||||
*re = r * hr - i * hi;
|
||||
*im = r * hi + i * hr;
|
||||
}
|
||||
|
||||
|
||||
int findUtmZone(double latitude_deg, double longitude_deg)
|
||||
{
|
||||
// Function finds the UTM zone number for given longitude and latitude.
|
||||
// The longitude value must be between -180 (180 degree West) and 180 (180
|
||||
// degree East) degree. The latitude must be within -80 (80 degree South) and
|
||||
// 84 (84 degree North).
|
||||
//
|
||||
// utmZone = findUtmZone(latitude, longitude);
|
||||
//
|
||||
// Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not
|
||||
// 15 deg 30 min).
|
||||
//
|
||||
// Originally written in Matlab by Darius Plausinaitis
|
||||
// Implemented in C++ by J.Arribas
|
||||
|
||||
// Check value bounds
|
||||
if ((longitude_deg > 180.0) || (longitude_deg < -180.0))
|
||||
std::cout << "Longitude value exceeds limits (-180:180).\n";
|
||||
|
||||
if ((latitude_deg > 84.0) || (latitude_deg < -80.0))
|
||||
std::cout << "Latitude value exceeds limits (-80:84).\n";
|
||||
|
||||
//
|
||||
// Find zone
|
||||
//
|
||||
|
||||
// Start at 180 deg west = -180 deg
|
||||
int utmZone = floor((180 + longitude_deg) / 6) + 1;
|
||||
|
||||
// Correct zone numbers for particular areas
|
||||
if (latitude_deg > 72.0)
|
||||
{
|
||||
// Corrections for zones 31 33 35 37
|
||||
if ((longitude_deg >= 0.0) && (longitude_deg < 9.0))
|
||||
{
|
||||
utmZone = 31;
|
||||
}
|
||||
else if ((longitude_deg >= 9.0) && (longitude_deg < 21.0))
|
||||
{
|
||||
utmZone = 33;
|
||||
}
|
||||
else if ((longitude_deg >= 21.0) && (longitude_deg < 33.0))
|
||||
{
|
||||
utmZone = 35;
|
||||
}
|
||||
else if ((longitude_deg >= 33.0) && (longitude_deg < 42.0))
|
||||
{
|
||||
utmZone = 37;
|
||||
}
|
||||
}
|
||||
else if ((latitude_deg >= 56.0) && (latitude_deg < 64.0))
|
||||
{
|
||||
// Correction for zone 32
|
||||
if ((longitude_deg >= 3.0) && (longitude_deg < 12.0))
|
||||
utmZone = 32;
|
||||
}
|
||||
return utmZone;
|
||||
}
|
||||
|
||||
@ -94,7 +94,7 @@ arma::mat Gravity_ECEF(const arma::vec &r_eb_e); //!< Calculates acceleration d
|
||||
*/
|
||||
arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection);
|
||||
|
||||
arma::vec LLH_to_deg(arma::vec &LLH);
|
||||
arma::vec LLH_to_deg(const arma::vec &LLH);
|
||||
|
||||
double degtorad(double angleInDegrees);
|
||||
|
||||
@ -104,7 +104,7 @@ double mstoknotsh(double MetersPerSeconds);
|
||||
|
||||
double mstokph(double Kph);
|
||||
|
||||
arma::vec CTM_to_Euler(arma::mat &C);
|
||||
arma::vec CTM_to_Euler(const arma::mat &C);
|
||||
|
||||
arma::mat Euler_to_CTM(const arma::vec &eul);
|
||||
|
||||
@ -151,4 +151,34 @@ void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat
|
||||
*/
|
||||
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e);
|
||||
|
||||
|
||||
/*!
|
||||
* \brief The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes.
|
||||
*/
|
||||
double great_circle_distance(double lat1, double lon1, double lat2, double lon2);
|
||||
|
||||
|
||||
/*!
|
||||
* \brief Transformation of ECEF (X,Y,Z) to (E,N,U) in UTM, zone 'zone'.
|
||||
*/
|
||||
void cart2utm(const arma::vec &r_eb_e, int zone, arma::vec &r_enu);
|
||||
|
||||
|
||||
/*!
|
||||
* \brief Function finds the UTM zone number for given longitude and latitude.
|
||||
*/
|
||||
int findUtmZone(double latitude_deg, double longitude_deg);
|
||||
|
||||
|
||||
/*!
|
||||
* \brief Clenshaw summation of sinus of argument.
|
||||
*/
|
||||
double clsin(const arma::colvec &ar, int degree, double argument);
|
||||
|
||||
|
||||
/*!
|
||||
* \brief Clenshaw summation of sinus with complex argument.
|
||||
*/
|
||||
void clksin(const arma::colvec &ar, int degree, double arg_real, double arg_imag, double *re, double *im);
|
||||
|
||||
#endif
|
||||
|
||||
@ -32,6 +32,7 @@
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "geofunctions.h"
|
||||
#include "position_test_flags.h"
|
||||
#include "rtklib_solver_dump_reader.h"
|
||||
#include "spirent_motion_csv_dump_reader.h"
|
||||
@ -54,6 +55,7 @@
|
||||
#include <fstream>
|
||||
#include <numeric>
|
||||
#include <thread>
|
||||
#include <armadillo>
|
||||
|
||||
// For GPS NAVIGATION (L1)
|
||||
concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue;
|
||||
@ -82,118 +84,13 @@ private:
|
||||
std::string filename_rinex_obs = FLAGS_filename_rinex_obs;
|
||||
std::string filename_raw_data = FLAGS_filename_raw_data;
|
||||
|
||||
void print_results(const std::vector<double>& east,
|
||||
const std::vector<double>& north,
|
||||
const std::vector<double>& up);
|
||||
|
||||
double compute_stdev_precision(const std::vector<double>& vec);
|
||||
double compute_stdev_accuracy(const std::vector<double>& vec, double ref);
|
||||
|
||||
void geodetic2Enu(const double latitude, const double longitude, const double altitude,
|
||||
double* east, double* north, double* up);
|
||||
|
||||
void geodetic2Ecef(const double latitude, const double longitude, const double altitude,
|
||||
double* x, double* y, double* z);
|
||||
|
||||
void print_results(arma::mat R_eb_enu);
|
||||
std::shared_ptr<InMemoryConfiguration> config;
|
||||
std::shared_ptr<FileConfiguration> config_f;
|
||||
std::string generated_kml_file;
|
||||
};
|
||||
|
||||
|
||||
void PositionSystemTest::geodetic2Ecef(const double latitude, const double longitude, const double altitude,
|
||||
double* x, double* y, double* z)
|
||||
{
|
||||
const double a = 6378137.0; // WGS84
|
||||
const double b = 6356752.314245; // WGS84
|
||||
|
||||
double aux_x, aux_y, aux_z;
|
||||
|
||||
// Convert to ECEF (See https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates )
|
||||
const double cLat = cos(latitude);
|
||||
const double cLon = cos(longitude);
|
||||
const double sLon = sin(longitude);
|
||||
const double sLat = sin(latitude);
|
||||
double N = std::pow(a, 2.0) / sqrt(std::pow(a, 2.0) * std::pow(cLat, 2.0) + std::pow(b, 2.0) * std::pow(sLat, 2.0));
|
||||
|
||||
aux_x = (N + altitude) * cLat * cLon;
|
||||
aux_y = (N + altitude) * cLat * sLon;
|
||||
aux_z = ((std::pow(b, 2.0) / std::pow(a, 2.0)) * N + altitude) * sLat;
|
||||
|
||||
*x = aux_x;
|
||||
*y = aux_y;
|
||||
*z = aux_z;
|
||||
}
|
||||
|
||||
|
||||
void PositionSystemTest::geodetic2Enu(double latitude, double longitude, double altitude,
|
||||
double* east, double* north, double* up)
|
||||
{
|
||||
double x, y, z;
|
||||
const double d2r = PI / 180.0;
|
||||
|
||||
geodetic2Ecef(latitude * d2r, longitude * d2r, altitude, &x, &y, &z);
|
||||
|
||||
double aux_north, aux_east, aux_down;
|
||||
|
||||
std::istringstream iss2(FLAGS_static_position);
|
||||
std::string str_aux;
|
||||
std::getline(iss2, str_aux, ',');
|
||||
double ref_long = std::stod(str_aux);
|
||||
std::getline(iss2, str_aux, ',');
|
||||
double ref_lat = std::stod(str_aux);
|
||||
std::getline(iss2, str_aux, '\n');
|
||||
double ref_h = std::stod(str_aux);
|
||||
double ref_x, ref_y, ref_z;
|
||||
|
||||
geodetic2Ecef(ref_lat * d2r, ref_long * d2r, ref_h, &ref_x, &ref_y, &ref_z);
|
||||
|
||||
double aux_x = x; // - ref_x;
|
||||
double aux_y = y; // - ref_y;
|
||||
double aux_z = z; // - ref_z;
|
||||
|
||||
// ECEF to NED matrix
|
||||
double phiP = atan2(ref_z, sqrt(std::pow(ref_x, 2.0) + std::pow(ref_y, 2.0)));
|
||||
const double sLat = sin(phiP);
|
||||
const double sLon = sin(ref_long * d2r);
|
||||
const double cLat = cos(phiP);
|
||||
const double cLon = cos(ref_long * d2r);
|
||||
|
||||
aux_north = -aux_x * sLat * cLon - aux_y * sLon + aux_z * cLat * cLon;
|
||||
aux_east = -aux_x * sLat * sLon + aux_y * cLon + aux_z * cLat * sLon;
|
||||
aux_down = aux_x * cLat + aux_z * sLat;
|
||||
|
||||
*east = aux_east;
|
||||
*north = aux_north;
|
||||
*up = -aux_down;
|
||||
}
|
||||
|
||||
|
||||
double PositionSystemTest::compute_stdev_precision(const std::vector<double>& vec)
|
||||
{
|
||||
double sum__ = std::accumulate(vec.begin(), vec.end(), 0.0);
|
||||
double mean__ = sum__ / vec.size();
|
||||
double accum__ = 0.0;
|
||||
std::for_each(std::begin(vec), std::end(vec), [&](const double d) {
|
||||
accum__ += (d - mean__) * (d - mean__);
|
||||
});
|
||||
double stdev__ = std::sqrt(accum__ / (vec.size() - 1));
|
||||
return stdev__;
|
||||
}
|
||||
|
||||
|
||||
double PositionSystemTest::compute_stdev_accuracy(const std::vector<double>& vec, const double ref)
|
||||
{
|
||||
const double mean__ = ref;
|
||||
double accum__ = 0.0;
|
||||
std::for_each(std::begin(vec), std::end(vec), [&](const double d) {
|
||||
accum__ += (d - mean__) * (d - mean__);
|
||||
});
|
||||
double stdev__ = std::sqrt(accum__ / (vec.size() - 1));
|
||||
return stdev__;
|
||||
}
|
||||
|
||||
|
||||
int PositionSystemTest::configure_generator()
|
||||
{
|
||||
// Configure signal generator
|
||||
@ -261,23 +158,23 @@ int PositionSystemTest::configure_receiver()
|
||||
const int grid_density = 16;
|
||||
|
||||
const float zero = 0.0;
|
||||
const int number_of_channels = 12;
|
||||
const int number_of_channels = 11;
|
||||
const int in_acquisition = 1;
|
||||
|
||||
const float threshold = 0.01;
|
||||
const float doppler_max = 8000.0;
|
||||
const float doppler_step = 500.0;
|
||||
const int max_dwells = 1;
|
||||
const float threshold = 2.5;
|
||||
const float doppler_max = 5000.0;
|
||||
const float doppler_step = 250.0;
|
||||
const int max_dwells = 10;
|
||||
const int tong_init_val = 2;
|
||||
const int tong_max_val = 10;
|
||||
const int tong_max_dwells = 30;
|
||||
const int coherent_integration_time_ms = 1;
|
||||
|
||||
const float pll_bw_hz = 30.0;
|
||||
const float dll_bw_hz = 4.0;
|
||||
const float pll_bw_hz = 35.0;
|
||||
const float dll_bw_hz = 1.5;
|
||||
const float early_late_space_chips = 0.5;
|
||||
const float pll_bw_narrow_hz = 20.0;
|
||||
const float dll_bw_narrow_hz = 2.0;
|
||||
const float pll_bw_narrow_hz = 1.0;
|
||||
const float dll_bw_narrow_hz = 0.1;
|
||||
const int extend_correlation_ms = 1;
|
||||
|
||||
const int display_rate_ms = 500;
|
||||
@ -307,7 +204,7 @@ int PositionSystemTest::configure_receiver()
|
||||
// Set the Signal Conditioner
|
||||
config->set_property("SignalConditioner.implementation", "Signal_Conditioner");
|
||||
config->set_property("DataTypeAdapter.implementation", "Ibyte_To_Complex");
|
||||
config->set_property("InputFilter.implementation", "Fir_Filter");
|
||||
config->set_property("InputFilter.implementation", "Freq_Xlating_Fir_Filter");
|
||||
config->set_property("InputFilter.dump", "false");
|
||||
config->set_property("InputFilter.input_item_type", "gr_complex");
|
||||
config->set_property("InputFilter.output_item_type", "gr_complex");
|
||||
@ -324,7 +221,7 @@ int PositionSystemTest::configure_receiver()
|
||||
config->set_property("InputFilter.ampl2_end", std::to_string(ampl2_end));
|
||||
config->set_property("InputFilter.band1_error", std::to_string(band1_error));
|
||||
config->set_property("InputFilter.band2_error", std::to_string(band2_error));
|
||||
config->set_property("InputFilter.filter_type", "bandpass");
|
||||
config->set_property("InputFilter.filter_type", "lowpass");
|
||||
config->set_property("InputFilter.grid_density", std::to_string(grid_density));
|
||||
config->set_property("InputFilter.sampling_frequency", std::to_string(sampling_rate_internal));
|
||||
config->set_property("InputFilter.IF", std::to_string(zero));
|
||||
@ -358,7 +255,6 @@ int PositionSystemTest::configure_receiver()
|
||||
|
||||
// Set Tracking
|
||||
config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_Tracking");
|
||||
//config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_C_Aid_Tracking");
|
||||
config->set_property("Tracking_1C.item_type", "gr_complex");
|
||||
config->set_property("Tracking_1C.dump", "false");
|
||||
config->set_property("Tracking_1C.dump_filename", "./tracking_ch_");
|
||||
@ -369,6 +265,8 @@ int PositionSystemTest::configure_receiver()
|
||||
config->set_property("Tracking_1C.pll_bw_narrow_hz", std::to_string(pll_bw_narrow_hz));
|
||||
config->set_property("Tracking_1C.dll_bw_narrow_hz", std::to_string(dll_bw_narrow_hz));
|
||||
config->set_property("Tracking_1C.extend_correlation_symbols", std::to_string(extend_correlation_ms));
|
||||
//config->set_property("Tracking_1C.high_dyn", "true");
|
||||
//config->set_property("Tracking_1C.smoother_length", "200");
|
||||
|
||||
// Set Telemetry
|
||||
config->set_property("TelemetryDecoder_1C.implementation", "GPS_L1_CA_Telemetry_Decoder");
|
||||
@ -381,7 +279,7 @@ int PositionSystemTest::configure_receiver()
|
||||
|
||||
// Set PVT
|
||||
config->set_property("PVT.implementation", "RTKLIB_PVT");
|
||||
config->set_property("PVT.positioning_mode", "Single");
|
||||
config->set_property("PVT.positioning_mode", "PPP_Static");
|
||||
config->set_property("PVT.output_rate_ms", std::to_string(output_rate_ms));
|
||||
config->set_property("PVT.display_rate_ms", std::to_string(display_rate_ms));
|
||||
config->set_property("PVT.dump_filename", "./PVT");
|
||||
@ -460,13 +358,10 @@ int PositionSystemTest::run_receiver()
|
||||
|
||||
void PositionSystemTest::check_results()
|
||||
{
|
||||
std::vector<double> pos_e;
|
||||
std::vector<double> pos_n;
|
||||
std::vector<double> pos_u;
|
||||
|
||||
arma::mat R_eb_e; //ECEF position (x,y,z) estimation in the Earth frame (Nx3)
|
||||
arma::mat V_eb_e; //ECEF velocity (x,y,z) estimation in the Earth frame (Nx3)
|
||||
arma::mat LLH; //Geodetic coordinates (latitude, longitude, height) estimation in WGS84 datum
|
||||
arma::mat R_eb_e; //ECEF position (x,y,z) estimation in the Earth frame (Nx3)
|
||||
arma::mat R_eb_enu; //ENU position (N,E,U) estimation in UTM (Nx3)
|
||||
arma::mat V_eb_e; //ECEF velocity (x,y,z) estimation in the Earth frame (Nx3)
|
||||
arma::mat LLH; //Geodetic coordinates (latitude, longitude, height) estimation in WGS84 datum
|
||||
arma::vec receiver_time_s;
|
||||
|
||||
arma::mat ref_R_eb_e; //ECEF position (x,y,z) reference in the Earth frame (Nx3)
|
||||
@ -482,10 +377,15 @@ void PositionSystemTest::check_results()
|
||||
double ref_long = std::stod(str_aux);
|
||||
std::getline(iss2, str_aux, '\n');
|
||||
double ref_h = std::stod(str_aux);
|
||||
double ref_e, ref_n, ref_u;
|
||||
geodetic2Enu(ref_lat, ref_long, ref_h,
|
||||
&ref_e, &ref_n, &ref_u);
|
||||
int utm_zone = findUtmZone(ref_lat, ref_long);
|
||||
|
||||
arma::vec v_eb_n = {0.0, 0.0, 0.0};
|
||||
arma::vec true_r_eb_e = {0.0, 0.0, 0.0};
|
||||
arma::vec true_v_eb_e = {0.0, 0.0, 0.0};
|
||||
pv_Geo_to_ECEF(degtorad(ref_lat), degtorad(ref_long), ref_h, v_eb_n, true_r_eb_e, true_v_eb_e);
|
||||
ref_R_eb_e.insert_cols(0, true_r_eb_e);
|
||||
arma::vec ref_r_enu = {0, 0, 0};
|
||||
cart2utm(true_r_eb_e, utm_zone, ref_r_enu);
|
||||
if (!FLAGS_use_pvt_solver_dump)
|
||||
{
|
||||
//fall back to read receiver KML output (position only)
|
||||
@ -503,8 +403,8 @@ void PositionSystemTest::check_results()
|
||||
if (found != std::string::npos) is_header = false;
|
||||
}
|
||||
bool is_data = true;
|
||||
|
||||
//read data
|
||||
int64_t current_epoch = 0;
|
||||
while (is_data)
|
||||
{
|
||||
if (!std::getline(myfile, line))
|
||||
@ -532,18 +432,20 @@ void PositionSystemTest::check_results()
|
||||
if (i == 2) h = value;
|
||||
}
|
||||
|
||||
double north, east, up;
|
||||
geodetic2Enu(lat, longitude, h, &east, &north, &up);
|
||||
arma::vec tmp_v_ecef;
|
||||
arma::vec tmp_r_ecef;
|
||||
pv_Geo_to_ECEF(degtorad(lat), degtorad(longitude), h, arma::vec{0, 0, 0}, tmp_r_ecef, tmp_v_ecef);
|
||||
R_eb_e.insert_cols(current_epoch, tmp_r_ecef);
|
||||
arma::vec tmp_r_enu = {0, 0, 0};
|
||||
cart2utm(tmp_r_ecef, utm_zone, tmp_r_enu);
|
||||
R_eb_enu.insert_cols(current_epoch, tmp_r_enu);
|
||||
// std::cout << "lat = " << lat << ", longitude = " << longitude << " h = " << h << std::endl;
|
||||
// std::cout << "E = " << east << ", N = " << north << " U = " << up << std::endl;
|
||||
pos_e.push_back(east);
|
||||
pos_n.push_back(north);
|
||||
pos_u.push_back(up);
|
||||
// getchar();
|
||||
}
|
||||
}
|
||||
myfile.close();
|
||||
ASSERT_FALSE(pos_e.size() == 0) << "KML file is empty";
|
||||
ASSERT_FALSE(R_eb_e.n_cols == 0) << "KML file is empty";
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -551,33 +453,27 @@ void PositionSystemTest::check_results()
|
||||
rtklib_solver_dump_reader pvt_reader;
|
||||
pvt_reader.open_obs_file(FLAGS_pvt_solver_dump_filename);
|
||||
int64_t n_epochs = pvt_reader.num_epochs();
|
||||
R_eb_e = arma::zeros(n_epochs, 3);
|
||||
V_eb_e = arma::zeros(n_epochs, 3);
|
||||
LLH = arma::zeros(n_epochs, 3);
|
||||
R_eb_e = arma::zeros(3, n_epochs);
|
||||
V_eb_e = arma::zeros(3, n_epochs);
|
||||
LLH = arma::zeros(3, n_epochs);
|
||||
receiver_time_s = arma::zeros(n_epochs, 1);
|
||||
int64_t current_epoch = 0;
|
||||
while (pvt_reader.read_binary_obs())
|
||||
{
|
||||
double north, east, up;
|
||||
geodetic2Enu(pvt_reader.latitude, pvt_reader.longitude, pvt_reader.height, &east, &north, &up);
|
||||
// std::cout << "lat = " << pvt_reader.latitude << ", longitude = " << pvt_reader.longitude << " h = " << pvt_reader.height << std::endl;
|
||||
// std::cout << "E = " << east << ", N = " << north << " U = " << up << std::endl;
|
||||
pos_e.push_back(east);
|
||||
pos_n.push_back(north);
|
||||
pos_u.push_back(up);
|
||||
// getchar();
|
||||
|
||||
// receiver_time_s(current_epoch) = static_cast<double>(pvt_reader.TOW_at_current_symbol_ms) / 1000.0;
|
||||
receiver_time_s(current_epoch) = pvt_reader.RX_time - pvt_reader.clk_offset_s;
|
||||
R_eb_e(current_epoch, 0) = pvt_reader.rr[0];
|
||||
R_eb_e(current_epoch, 1) = pvt_reader.rr[1];
|
||||
R_eb_e(current_epoch, 2) = pvt_reader.rr[2];
|
||||
V_eb_e(current_epoch, 0) = pvt_reader.rr[3];
|
||||
V_eb_e(current_epoch, 1) = pvt_reader.rr[4];
|
||||
V_eb_e(current_epoch, 2) = pvt_reader.rr[5];
|
||||
LLH(current_epoch, 0) = pvt_reader.latitude;
|
||||
LLH(current_epoch, 1) = pvt_reader.longitude;
|
||||
LLH(current_epoch, 2) = pvt_reader.height;
|
||||
R_eb_e(0, current_epoch) = pvt_reader.rr[0];
|
||||
R_eb_e(1, current_epoch) = pvt_reader.rr[1];
|
||||
R_eb_e(2, current_epoch) = pvt_reader.rr[2];
|
||||
V_eb_e(0, current_epoch) = pvt_reader.rr[3];
|
||||
V_eb_e(1, current_epoch) = pvt_reader.rr[4];
|
||||
V_eb_e(2, current_epoch) = pvt_reader.rr[5];
|
||||
LLH(0, current_epoch) = pvt_reader.latitude;
|
||||
LLH(1, current_epoch) = pvt_reader.longitude;
|
||||
LLH(2, current_epoch) = pvt_reader.height;
|
||||
|
||||
arma::vec tmp_r_enu = {0, 0, 0};
|
||||
cart2utm(R_eb_e.col(current_epoch), utm_zone, tmp_r_enu);
|
||||
R_eb_enu.insert_cols(current_epoch, tmp_r_enu);
|
||||
|
||||
//debug check
|
||||
// std::cout << "t1: " << pvt_reader.RX_time << std::endl;
|
||||
@ -593,20 +489,21 @@ void PositionSystemTest::check_results()
|
||||
|
||||
if (FLAGS_static_scenario)
|
||||
{
|
||||
double sigma_E_2_precision = std::pow(compute_stdev_precision(pos_e), 2.0);
|
||||
double sigma_N_2_precision = std::pow(compute_stdev_precision(pos_n), 2.0);
|
||||
double sigma_U_2_precision = std::pow(compute_stdev_precision(pos_u), 2.0);
|
||||
double sigma_E_2_precision = arma::var(R_eb_enu.row(0));
|
||||
double sigma_N_2_precision = arma::var(R_eb_enu.row(1));
|
||||
double sigma_U_2_precision = arma::var(R_eb_enu.row(2));
|
||||
|
||||
double sigma_E_2_accuracy = std::pow(compute_stdev_accuracy(pos_e, ref_e), 2.0);
|
||||
double sigma_N_2_accuracy = std::pow(compute_stdev_accuracy(pos_n, ref_n), 2.0);
|
||||
double sigma_U_2_accuracy = std::pow(compute_stdev_accuracy(pos_u, ref_u), 2.0);
|
||||
arma::rowvec tmp_vec;
|
||||
tmp_vec = R_eb_enu.row(0) - ref_r_enu(0);
|
||||
double sigma_E_2_accuracy = sqrt(arma::sum(arma::square(tmp_vec)) / tmp_vec.n_cols);
|
||||
tmp_vec = R_eb_enu.row(1) - ref_r_enu(1);
|
||||
double sigma_N_2_accuracy = sqrt(arma::sum(arma::square(tmp_vec)) / tmp_vec.n_cols);
|
||||
tmp_vec = R_eb_enu.row(2) - ref_r_enu(2);
|
||||
double sigma_U_2_accuracy = sqrt(arma::sum(arma::square(tmp_vec)) / tmp_vec.n_cols);
|
||||
|
||||
double sum__e = std::accumulate(pos_e.begin(), pos_e.end(), 0.0);
|
||||
double mean__e = sum__e / pos_e.size();
|
||||
double sum__n = std::accumulate(pos_n.begin(), pos_n.end(), 0.0);
|
||||
double mean__n = sum__n / pos_n.size();
|
||||
double sum__u = std::accumulate(pos_u.begin(), pos_u.end(), 0.0);
|
||||
double mean__u = sum__u / pos_u.size();
|
||||
double mean__e = arma::mean(R_eb_enu.row(0));
|
||||
double mean__n = arma::mean(R_eb_enu.row(1));
|
||||
double mean__u = arma::mean(R_eb_enu.row(2));
|
||||
|
||||
std::stringstream stm;
|
||||
std::ofstream position_test_file;
|
||||
@ -614,25 +511,23 @@ void PositionSystemTest::check_results()
|
||||
{
|
||||
stm << "Configuration file: " << FLAGS_config_file_ptest << std::endl;
|
||||
}
|
||||
if (FLAGS_static_scenario)
|
||||
{
|
||||
stm << "---- ACCURACY ----" << std::endl;
|
||||
stm << "2DRMS = " << 2 * sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl;
|
||||
stm << "DRMS = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl;
|
||||
stm << "CEP = " << 0.62 * compute_stdev_accuracy(pos_n, ref_n) + 0.56 * compute_stdev_accuracy(pos_e, ref_e) << " [m]" << std::endl;
|
||||
stm << "99% SAS = " << 1.122 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
|
||||
stm << "90% SAS = " << 0.833 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
|
||||
stm << "MRSE = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
|
||||
stm << "SEP = " << 0.51 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
|
||||
stm << "Bias 2D = " << sqrt(std::pow(abs(mean__e - ref_e), 2.0) + std::pow(abs(mean__n - ref_n), 2.0)) << " [m]" << std::endl;
|
||||
stm << "Bias 3D = " << sqrt(std::pow(abs(mean__e - ref_e), 2.0) + std::pow(abs(mean__n - ref_n), 2.0) + std::pow(abs(mean__u - ref_u), 2.0)) << " [m]" << std::endl;
|
||||
stm << std::endl;
|
||||
}
|
||||
|
||||
stm << "---- ACCURACY ----" << std::endl;
|
||||
stm << "2DRMS = " << 2 * sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl;
|
||||
stm << "DRMS = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl;
|
||||
stm << "CEP = " << 0.62 * sqrt(sigma_N_2_accuracy) + 0.56 * sqrt(sigma_E_2_accuracy) << " [m]" << std::endl;
|
||||
stm << "99% SAS = " << 1.122 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
|
||||
stm << "90% SAS = " << 0.833 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
|
||||
stm << "MRSE = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
|
||||
stm << "SEP = " << 0.51 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
|
||||
stm << "Bias 2D = " << sqrt(std::pow(fabs(mean__e - ref_r_enu(0)), 2.0) + std::pow(fabs(mean__n - ref_r_enu(1)), 2.0)) << " [m]" << std::endl;
|
||||
stm << "Bias 3D = " << sqrt(std::pow(fabs(mean__e - ref_r_enu(0)), 2.0) + std::pow(fabs(mean__n - ref_r_enu(1)), 2.0) + std::pow(fabs(mean__u - ref_r_enu(2)), 2.0)) << " [m]" << std::endl;
|
||||
stm << std::endl;
|
||||
|
||||
stm << "---- PRECISION ----" << std::endl;
|
||||
stm << "2DRMS = " << 2 * sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl;
|
||||
stm << "DRMS = " << sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl;
|
||||
stm << "CEP = " << 0.62 * compute_stdev_precision(pos_n) + 0.56 * compute_stdev_precision(pos_e) << " [m]" << std::endl;
|
||||
stm << "CEP = " << 0.62 * sqrt(sigma_N_2_precision) + 0.56 * sqrt(sigma_E_2_precision) << " [m]" << std::endl;
|
||||
stm << "99% SAS = " << 1.122 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
|
||||
stm << "90% SAS = " << 0.833 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
|
||||
stm << "MRSE = " << sqrt(sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
|
||||
@ -649,11 +544,11 @@ void PositionSystemTest::check_results()
|
||||
|
||||
// Sanity Check
|
||||
double precision_SEP = 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision);
|
||||
ASSERT_LT(precision_SEP, 20.0);
|
||||
ASSERT_LT(precision_SEP, 1.0);
|
||||
|
||||
if (FLAGS_plot_position_test == true)
|
||||
{
|
||||
print_results(pos_e, pos_n, pos_u);
|
||||
print_results(R_eb_enu);
|
||||
}
|
||||
}
|
||||
else
|
||||
@ -662,56 +557,55 @@ void PositionSystemTest::check_results()
|
||||
spirent_motion_csv_dump_reader ref_reader;
|
||||
ref_reader.open_obs_file(FLAGS_ref_motion_filename);
|
||||
int64_t n_epochs = ref_reader.num_epochs();
|
||||
ref_R_eb_e = arma::zeros(n_epochs, 3);
|
||||
ref_V_eb_e = arma::zeros(n_epochs, 3);
|
||||
ref_LLH = arma::zeros(n_epochs, 3);
|
||||
ref_R_eb_e = arma::zeros(3, n_epochs);
|
||||
ref_V_eb_e = arma::zeros(3, n_epochs);
|
||||
ref_LLH = arma::zeros(3, n_epochs);
|
||||
ref_time_s = arma::zeros(n_epochs, 1);
|
||||
int64_t current_epoch = 0;
|
||||
while (ref_reader.read_csv_obs())
|
||||
{
|
||||
ref_time_s(current_epoch) = ref_reader.TOW_ms / 1000.0;
|
||||
ref_R_eb_e(current_epoch, 0) = ref_reader.Pos_X;
|
||||
ref_R_eb_e(current_epoch, 1) = ref_reader.Pos_Y;
|
||||
ref_R_eb_e(current_epoch, 2) = ref_reader.Pos_Z;
|
||||
ref_V_eb_e(current_epoch, 0) = ref_reader.Vel_X;
|
||||
ref_V_eb_e(current_epoch, 1) = ref_reader.Vel_Y;
|
||||
ref_V_eb_e(current_epoch, 2) = ref_reader.Vel_Z;
|
||||
ref_LLH(current_epoch, 0) = ref_reader.Lat;
|
||||
ref_LLH(current_epoch, 1) = ref_reader.Long;
|
||||
ref_LLH(current_epoch, 2) = ref_reader.Height;
|
||||
ref_R_eb_e(0, current_epoch) = ref_reader.Pos_X;
|
||||
ref_R_eb_e(1, current_epoch) = ref_reader.Pos_Y;
|
||||
ref_R_eb_e(2, current_epoch) = ref_reader.Pos_Z;
|
||||
ref_V_eb_e(0, current_epoch) = ref_reader.Vel_X;
|
||||
ref_V_eb_e(1, current_epoch) = ref_reader.Vel_Y;
|
||||
ref_V_eb_e(2, current_epoch) = ref_reader.Vel_Z;
|
||||
ref_LLH(0, current_epoch) = ref_reader.Lat;
|
||||
ref_LLH(1, current_epoch) = ref_reader.Long;
|
||||
ref_LLH(2, current_epoch) = ref_reader.Height;
|
||||
current_epoch++;
|
||||
}
|
||||
|
||||
//interpolation of reference data to receiver epochs timestamps
|
||||
arma::mat ref_interp_R_eb_e = arma::zeros(R_eb_e.n_rows, 3);
|
||||
arma::mat ref_interp_V_eb_e = arma::zeros(V_eb_e.n_rows, 3);
|
||||
arma::mat ref_interp_LLH = arma::zeros(LLH.n_rows, 3);
|
||||
arma::mat ref_interp_R_eb_e = arma::zeros(3, R_eb_e.n_cols);
|
||||
arma::mat ref_interp_V_eb_e = arma::zeros(3, V_eb_e.n_cols);
|
||||
arma::mat ref_interp_LLH = arma::zeros(3, LLH.n_cols);
|
||||
arma::vec tmp_vector;
|
||||
for (int n = 0; n < 3; n++)
|
||||
{
|
||||
arma::interp1(ref_time_s, ref_R_eb_e.col(n), receiver_time_s, tmp_vector);
|
||||
ref_interp_R_eb_e.col(n) = tmp_vector;
|
||||
arma::interp1(ref_time_s, ref_V_eb_e.col(n), receiver_time_s, tmp_vector);
|
||||
ref_interp_V_eb_e.col(n) = tmp_vector;
|
||||
arma::interp1(ref_time_s, ref_LLH.col(n), receiver_time_s, tmp_vector);
|
||||
ref_interp_LLH.col(n) = tmp_vector;
|
||||
arma::interp1(ref_time_s, ref_R_eb_e.row(n), receiver_time_s, tmp_vector);
|
||||
ref_interp_R_eb_e.row(n) = tmp_vector.t();
|
||||
arma::interp1(ref_time_s, ref_V_eb_e.row(n), receiver_time_s, tmp_vector);
|
||||
ref_interp_V_eb_e.row(n) = tmp_vector.t();
|
||||
arma::interp1(ref_time_s, ref_LLH.row(n), receiver_time_s, tmp_vector);
|
||||
ref_interp_LLH.row(n) = tmp_vector.t();
|
||||
}
|
||||
|
||||
//compute error vectors
|
||||
|
||||
arma::mat error_R_eb_e = arma::zeros(R_eb_e.n_rows, 3);
|
||||
arma::mat error_V_eb_e = arma::zeros(V_eb_e.n_rows, 3);
|
||||
arma::mat error_LLH = arma::zeros(LLH.n_rows, 3);
|
||||
arma::mat error_R_eb_e = arma::zeros(3, R_eb_e.n_cols);
|
||||
arma::mat error_V_eb_e = arma::zeros(3, V_eb_e.n_cols);
|
||||
arma::mat error_LLH = arma::zeros(3, LLH.n_cols);
|
||||
error_R_eb_e = R_eb_e - ref_interp_R_eb_e;
|
||||
error_V_eb_e = V_eb_e - ref_interp_V_eb_e;
|
||||
error_LLH = LLH - ref_interp_LLH;
|
||||
arma::vec error_module_R_eb_e = arma::zeros(R_eb_e.n_rows, 1);
|
||||
arma::vec error_module_V_eb_e = arma::zeros(V_eb_e.n_rows, 1);
|
||||
for (uint64_t n = 0; n < R_eb_e.n_rows; n++)
|
||||
arma::vec error_module_R_eb_e = arma::zeros(R_eb_e.n_cols, 1);
|
||||
arma::vec error_module_V_eb_e = arma::zeros(V_eb_e.n_cols, 1);
|
||||
for (uint64_t n = 0; n < R_eb_e.n_cols; n++)
|
||||
{
|
||||
error_module_R_eb_e(n) = arma::norm(error_R_eb_e.row(n));
|
||||
error_module_V_eb_e(n) = arma::norm(error_V_eb_e.row(n));
|
||||
error_module_R_eb_e(n) = arma::norm(error_R_eb_e.col(n));
|
||||
error_module_V_eb_e(n) = arma::norm(error_V_eb_e.col(n));
|
||||
}
|
||||
|
||||
//Error statistics
|
||||
arma::vec tmp_vec;
|
||||
//RMSE, Mean, Variance and peaks
|
||||
@ -750,103 +644,112 @@ void PositionSystemTest::check_results()
|
||||
<< " [m/s]" << std::endl;
|
||||
std::cout.precision(ss);
|
||||
|
||||
//plots
|
||||
Gnuplot g1("points");
|
||||
if (FLAGS_show_plots)
|
||||
// plots
|
||||
if (FLAGS_plot_position_test == true)
|
||||
{
|
||||
g1.showonscreen(); // window output
|
||||
}
|
||||
else
|
||||
{
|
||||
g1.disablescreen();
|
||||
}
|
||||
g1.set_title("3D ECEF error coordinates");
|
||||
g1.set_grid();
|
||||
//conversion between arma::vec and std:vector
|
||||
std::vector<double> X(error_R_eb_e.colptr(0), error_R_eb_e.colptr(0) + error_R_eb_e.n_rows);
|
||||
std::vector<double> Y(error_R_eb_e.colptr(1), error_R_eb_e.colptr(1) + error_R_eb_e.n_rows);
|
||||
std::vector<double> Z(error_R_eb_e.colptr(2), error_R_eb_e.colptr(2) + error_R_eb_e.n_rows);
|
||||
const std::string gnuplot_executable(FLAGS_gnuplot_executable);
|
||||
if (!gnuplot_executable.empty())
|
||||
{
|
||||
Gnuplot g1("points");
|
||||
if (FLAGS_show_plots)
|
||||
{
|
||||
g1.showonscreen(); // window output
|
||||
}
|
||||
else
|
||||
{
|
||||
g1.disablescreen();
|
||||
}
|
||||
g1.set_title("3D ECEF error coordinates");
|
||||
g1.set_grid();
|
||||
//conversion between arma::vec and std:vector
|
||||
arma::rowvec arma_vec_error_x = error_R_eb_e.row(0);
|
||||
arma::rowvec arma_vec_error_y = error_R_eb_e.row(1);
|
||||
arma::rowvec arma_vec_error_z = error_R_eb_e.row(2);
|
||||
|
||||
g1.cmd("set key box opaque");
|
||||
g1.plot_xyz(X, Y, Z, "ECEF 3D error");
|
||||
g1.set_legend();
|
||||
if (FLAGS_config_file_ptest.empty())
|
||||
{
|
||||
g1.savetops("ECEF_3d_error");
|
||||
}
|
||||
else
|
||||
{
|
||||
g1.savetops("ECEF_3d_error_" + config_filename_no_extension);
|
||||
}
|
||||
arma::vec time_vector_from_start_s = receiver_time_s - receiver_time_s(0);
|
||||
Gnuplot g3("linespoints");
|
||||
if (FLAGS_show_plots)
|
||||
{
|
||||
g3.showonscreen(); // window output
|
||||
}
|
||||
else
|
||||
{
|
||||
g3.disablescreen();
|
||||
}
|
||||
g3.set_title("3D Position estimation error module [m]");
|
||||
g3.set_grid();
|
||||
g3.set_xlabel("Receiver epoch time from first valid PVT [s]");
|
||||
g3.set_ylabel("3D Position error [m]");
|
||||
//conversion between arma::vec and std:vector
|
||||
std::vector<double> error_vec(error_module_R_eb_e.colptr(0), error_module_R_eb_e.colptr(0) + error_module_R_eb_e.n_rows);
|
||||
g3.cmd("set key box opaque");
|
||||
g3.plot_xy(time_vector_from_start_s, error_vec, "Position 3D error");
|
||||
double mean3d = std::accumulate(error_vec.begin(), error_vec.end(), 0.0) / error_vec.size();
|
||||
std::vector<double> error_mean(error_module_R_eb_e.n_rows, mean3d);
|
||||
g3.set_style("lines");
|
||||
g3.plot_xy(time_vector_from_start_s, error_mean, "Mean");
|
||||
g3.set_legend();
|
||||
if (FLAGS_config_file_ptest.empty())
|
||||
{
|
||||
g3.savetops("Position_3d_error");
|
||||
}
|
||||
else
|
||||
{
|
||||
g3.savetops("Position_3d_error_" + config_filename_no_extension);
|
||||
}
|
||||
std::vector<double> X(arma_vec_error_x.colptr(0), arma_vec_error_x.colptr(0) + arma_vec_error_x.n_rows);
|
||||
std::vector<double> Y(arma_vec_error_y.colptr(0), arma_vec_error_y.colptr(0) + arma_vec_error_y.n_rows);
|
||||
std::vector<double> Z(arma_vec_error_z.colptr(0), arma_vec_error_z.colptr(0) + arma_vec_error_z.n_rows);
|
||||
|
||||
Gnuplot g4("linespoints");
|
||||
if (FLAGS_show_plots)
|
||||
{
|
||||
g4.showonscreen(); // window output
|
||||
}
|
||||
else
|
||||
{
|
||||
g4.disablescreen();
|
||||
}
|
||||
g4.set_title("3D Velocity estimation error module [m/s]");
|
||||
g4.set_grid();
|
||||
g4.set_xlabel("Receiver epoch time from first valid PVT [s]");
|
||||
g4.set_ylabel("3D Velocity error [m/s]");
|
||||
//conversion between arma::vec and std:vector
|
||||
std::vector<double> error_vec2(error_module_V_eb_e.colptr(0), error_module_V_eb_e.colptr(0) + error_module_V_eb_e.n_rows);
|
||||
g4.cmd("set key box opaque");
|
||||
g4.plot_xy(time_vector_from_start_s, error_vec2, "Velocity 3D error");
|
||||
double mean3dv = std::accumulate(error_vec2.begin(), error_vec2.end(), 0.0) / error_vec2.size();
|
||||
std::vector<double> error_mean_v(error_module_V_eb_e.n_rows, mean3dv);
|
||||
g4.set_style("lines");
|
||||
g4.plot_xy(time_vector_from_start_s, error_mean_v, "Mean");
|
||||
g4.set_legend();
|
||||
if (FLAGS_config_file_ptest.empty())
|
||||
{
|
||||
g4.savetops("Velocity_3d_error");
|
||||
}
|
||||
else
|
||||
{
|
||||
g4.savetops("Velocity_3d_error_" + config_filename_no_extension);
|
||||
g1.cmd("set key box opaque");
|
||||
g1.plot_xyz(X, Y, Z, "ECEF 3D error");
|
||||
g1.set_legend();
|
||||
if (FLAGS_config_file_ptest.empty())
|
||||
{
|
||||
g1.savetops("ECEF_3d_error");
|
||||
}
|
||||
else
|
||||
{
|
||||
g1.savetops("ECEF_3d_error_" + config_filename_no_extension);
|
||||
}
|
||||
arma::vec time_vector_from_start_s = receiver_time_s - receiver_time_s(0);
|
||||
Gnuplot g3("linespoints");
|
||||
if (FLAGS_show_plots)
|
||||
{
|
||||
g3.showonscreen(); // window output
|
||||
}
|
||||
else
|
||||
{
|
||||
g3.disablescreen();
|
||||
}
|
||||
g3.set_title("3D Position estimation error module [m]");
|
||||
g3.set_grid();
|
||||
g3.set_xlabel("Receiver epoch time from first valid PVT [s]");
|
||||
g3.set_ylabel("3D Position error [m]");
|
||||
//conversion between arma::vec and std:vector
|
||||
std::vector<double> error_vec(error_module_R_eb_e.colptr(0), error_module_R_eb_e.colptr(0) + error_module_R_eb_e.n_rows);
|
||||
g3.cmd("set key box opaque");
|
||||
g3.plot_xy(time_vector_from_start_s, error_vec, "Position 3D error");
|
||||
double mean3d = std::accumulate(error_vec.begin(), error_vec.end(), 0.0) / error_vec.size();
|
||||
std::vector<double> error_mean(error_module_R_eb_e.n_rows, mean3d);
|
||||
g3.set_style("lines");
|
||||
g3.plot_xy(time_vector_from_start_s, error_mean, "Mean");
|
||||
g3.set_legend();
|
||||
if (FLAGS_config_file_ptest.empty())
|
||||
{
|
||||
g3.savetops("Position_3d_error");
|
||||
}
|
||||
else
|
||||
{
|
||||
g3.savetops("Position_3d_error_" + config_filename_no_extension);
|
||||
}
|
||||
|
||||
Gnuplot g4("linespoints");
|
||||
if (FLAGS_show_plots)
|
||||
{
|
||||
g4.showonscreen(); // window output
|
||||
}
|
||||
else
|
||||
{
|
||||
g4.disablescreen();
|
||||
}
|
||||
g4.set_title("3D Velocity estimation error module [m/s]");
|
||||
g4.set_grid();
|
||||
g4.set_xlabel("Receiver epoch time from first valid PVT [s]");
|
||||
g4.set_ylabel("3D Velocity error [m/s]");
|
||||
//conversion between arma::vec and std:vector
|
||||
std::vector<double> error_vec2(error_module_V_eb_e.colptr(0), error_module_V_eb_e.colptr(0) + error_module_V_eb_e.n_rows);
|
||||
g4.cmd("set key box opaque");
|
||||
g4.plot_xy(time_vector_from_start_s, error_vec2, "Velocity 3D error");
|
||||
double mean3dv = std::accumulate(error_vec2.begin(), error_vec2.end(), 0.0) / error_vec2.size();
|
||||
std::vector<double> error_mean_v(error_module_V_eb_e.n_rows, mean3dv);
|
||||
g4.set_style("lines");
|
||||
g4.plot_xy(time_vector_from_start_s, error_mean_v, "Mean");
|
||||
g4.set_legend();
|
||||
if (FLAGS_config_file_ptest.empty())
|
||||
{
|
||||
g4.savetops("Velocity_3d_error");
|
||||
}
|
||||
else
|
||||
{
|
||||
g4.savetops("Velocity_3d_error_" + config_filename_no_extension);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void PositionSystemTest::print_results(const std::vector<double>& east,
|
||||
const std::vector<double>& north,
|
||||
const std::vector<double>& up)
|
||||
void PositionSystemTest::print_results(arma::mat R_eb_enu)
|
||||
{
|
||||
const std::string gnuplot_executable(FLAGS_gnuplot_executable);
|
||||
if (gnuplot_executable.empty())
|
||||
@ -857,29 +760,40 @@ void PositionSystemTest::print_results(const std::vector<double>& east,
|
||||
}
|
||||
else
|
||||
{
|
||||
double sigma_E_2_precision = std::pow(compute_stdev_precision(east), 2.0);
|
||||
double sigma_N_2_precision = std::pow(compute_stdev_precision(north), 2.0);
|
||||
double sigma_U_2_precision = std::pow(compute_stdev_precision(up), 2.0);
|
||||
double sigma_E_2_precision = arma::var(R_eb_enu.row(0));
|
||||
double sigma_N_2_precision = arma::var(R_eb_enu.row(1));
|
||||
double sigma_U_2_precision = arma::var(R_eb_enu.row(2));
|
||||
|
||||
double mean_east = std::accumulate(east.begin(), east.end(), 0.0) / east.size();
|
||||
double mean_north = std::accumulate(north.begin(), north.end(), 0.0) / north.size();
|
||||
double mean_east = arma::mean(R_eb_enu.row(0));
|
||||
double mean_north = arma::mean(R_eb_enu.row(1));
|
||||
double mean_up = arma::mean(R_eb_enu.row(2));
|
||||
|
||||
auto it_max_east = std::max_element(std::begin(east), std::end(east));
|
||||
auto it_min_east = std::min_element(std::begin(east), std::end(east));
|
||||
auto it_max_north = std::max_element(std::begin(north), std::end(north));
|
||||
auto it_min_north = std::min_element(std::begin(north), std::end(north));
|
||||
auto it_max_up = std::max_element(std::begin(up), std::end(up));
|
||||
auto it_min_up = std::min_element(std::begin(up), std::end(up));
|
||||
double it_max_east = arma::max(R_eb_enu.row(0) - mean_east);
|
||||
double it_min_east = arma::min(R_eb_enu.row(0) - mean_east);
|
||||
|
||||
auto east_range = std::max(*it_max_east, std::abs(*it_min_east));
|
||||
auto north_range = std::max(*it_max_north, std::abs(*it_min_north));
|
||||
auto up_range = std::max(*it_max_up, std::abs(*it_min_up));
|
||||
double it_max_north = arma::max(R_eb_enu.row(1) - mean_north);
|
||||
double it_min_north = arma::min(R_eb_enu.row(1) - mean_north);
|
||||
|
||||
double it_max_up = arma::max(R_eb_enu.row(2) - mean_up);
|
||||
double it_min_up = arma::min(R_eb_enu.row(2) - mean_up);
|
||||
|
||||
double east_range = std::max(it_max_east, std::abs(it_min_east));
|
||||
double north_range = std::max(it_max_north, std::abs(it_min_north));
|
||||
double up_range = std::max(it_max_up, std::abs(it_min_up));
|
||||
|
||||
double range = std::max(east_range, north_range) * 1.1;
|
||||
double range_3d = std::max(std::max(east_range, north_range), up_range) * 1.1;
|
||||
|
||||
double two_drms = 2 * sqrt(sigma_E_2_precision + sigma_N_2_precision);
|
||||
double ninty_sas = 0.833 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision);
|
||||
arma::rowvec arma_east = R_eb_enu.row(0) - mean_east;
|
||||
arma::rowvec arma_north = R_eb_enu.row(1) - mean_north;
|
||||
arma::rowvec arma_up = R_eb_enu.row(2) - mean_up;
|
||||
|
||||
std::vector<double> east(arma_east.colptr(0), arma_east.row(0).colptr(0) + arma_east.row(0).n_cols);
|
||||
std::vector<double> north(arma_north.colptr(0), arma_north.colptr(0) + arma_north.n_cols);
|
||||
std::vector<double> up(arma_up.colptr(0), arma_up.colptr(0) + arma_up.n_cols);
|
||||
|
||||
try
|
||||
{
|
||||
boost::filesystem::path p(gnuplot_executable);
|
||||
@ -903,6 +817,7 @@ void PositionSystemTest::print_results(const std::vector<double>& east,
|
||||
g1.cmd("set xrange [-" + std::to_string(range) + ":" + std::to_string(range) + "]");
|
||||
g1.cmd("set yrange [-" + std::to_string(range) + ":" + std::to_string(range) + "]");
|
||||
|
||||
|
||||
g1.plot_xy(east, north, "2D Position Fixes");
|
||||
g1.set_style("lines").plot_circle(mean_east, mean_north, two_drms, "2DRMS");
|
||||
g1.set_style("lines").plot_circle(mean_east, mean_north, two_drms / 2.0, "DRMS");
|
||||
|
||||
@ -45,7 +45,9 @@ bool tracking_dump_reader::read_binary_obs()
|
||||
d_dump_file.read(reinterpret_cast<char *>(&PRN_start_sample_count), sizeof(uint64_t));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&acc_carrier_phase_rad), sizeof(float));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&carrier_doppler_hz), sizeof(float));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&carrier_doppler_rate_hz_s), sizeof(float));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&code_freq_chips), sizeof(float));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&code_freq_rate_chips), sizeof(float));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&carr_error_hz), sizeof(float));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&carr_error_filt_hz), sizeof(float));
|
||||
d_dump_file.read(reinterpret_cast<char *>(&code_error_chips), sizeof(float));
|
||||
@ -83,7 +85,7 @@ int64_t tracking_dump_reader::num_epochs()
|
||||
{
|
||||
std::ifstream::pos_type size;
|
||||
int number_of_double_vars = 1;
|
||||
int number_of_float_vars = 17;
|
||||
int number_of_float_vars = 19;
|
||||
int epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
|
||||
sizeof(float) * number_of_float_vars + sizeof(unsigned int);
|
||||
std::ifstream tmpfile(d_dump_filename.c_str(), std::ios::binary | std::ios::ate);
|
||||
|
||||
@ -63,7 +63,9 @@ public:
|
||||
|
||||
// carrier and code frequency
|
||||
float carrier_doppler_hz;
|
||||
float carrier_doppler_rate_hz_s;
|
||||
float code_freq_chips;
|
||||
float code_freq_rate_chips;
|
||||
|
||||
// PLL commands
|
||||
float carr_error_hz;
|
||||
|
||||
@ -239,6 +239,7 @@ public:
|
||||
void check_results_duplicated_satellite(
|
||||
arma::mat& measured_sat1,
|
||||
arma::mat& measured_sat2,
|
||||
int ch_id,
|
||||
std::string data_title);
|
||||
|
||||
HybridObservablesTest()
|
||||
@ -260,7 +261,9 @@ public:
|
||||
double DLL_wide_bw_hz,
|
||||
double PLL_narrow_bw_hz,
|
||||
double DLL_narrow_bw_hz,
|
||||
int extend_correlation_symbols);
|
||||
int extend_correlation_symbols,
|
||||
uint32_t smoother_length,
|
||||
bool high_dyn);
|
||||
|
||||
gr::top_block_sptr top_block;
|
||||
std::shared_ptr<GNSSBlockFactory> factory;
|
||||
@ -540,10 +543,17 @@ void HybridObservablesTest::configure_receiver(
|
||||
double DLL_wide_bw_hz,
|
||||
double PLL_narrow_bw_hz,
|
||||
double DLL_narrow_bw_hz,
|
||||
int extend_correlation_symbols)
|
||||
int extend_correlation_symbols,
|
||||
uint32_t smoother_length,
|
||||
bool high_dyn)
|
||||
{
|
||||
config = std::make_shared<InMemoryConfiguration>();
|
||||
config->set_property("Tracking.dump", "true");
|
||||
if (high_dyn)
|
||||
config->set_property("Tracking.high_dyn", "true");
|
||||
else
|
||||
config->set_property("Tracking.high_dyn", "false");
|
||||
config->set_property("Tracking.smoother_length", std::to_string(smoother_length));
|
||||
config->set_property("Tracking.dump_filename", "./tracking_ch_");
|
||||
config->set_property("Tracking.implementation", implementation);
|
||||
config->set_property("Tracking.item_type", "gr_complex");
|
||||
@ -650,6 +660,8 @@ void HybridObservablesTest::configure_receiver(
|
||||
std::cout << "pll_bw_narrow_hz: " << config->property("Tracking.pll_bw_narrow_hz", 0.0) << " Hz\n";
|
||||
std::cout << "dll_bw_narrow_hz: " << config->property("Tracking.dll_bw_narrow_hz", 0.0) << " Hz\n";
|
||||
std::cout << "extend_correlation_symbols: " << config->property("Tracking.extend_correlation_symbols", 0) << " Symbols\n";
|
||||
std::cout << "high_dyn: " << config->property("Tracking.high_dyn", false) << "\n";
|
||||
std::cout << "smoother_length: " << config->property("Tracking.smoother_length", 0) << "\n";
|
||||
std::cout << "*****************************************\n";
|
||||
std::cout << "*****************************************\n";
|
||||
}
|
||||
@ -995,13 +1007,40 @@ void HybridObservablesTest::check_results_carrier_doppler(
|
||||
void HybridObservablesTest::check_results_duplicated_satellite(
|
||||
arma::mat& measured_sat1,
|
||||
arma::mat& measured_sat2,
|
||||
int ch_id,
|
||||
std::string data_title)
|
||||
{
|
||||
//1. True value interpolation to match the measurement times
|
||||
|
||||
double t0 = measured_sat1(0, 0);
|
||||
//define the common measured time interval
|
||||
double t0_sat1 = measured_sat1(0, 0);
|
||||
int size1 = measured_sat1.col(0).n_rows;
|
||||
double t1 = measured_sat1(size1 - 1, 0);
|
||||
double t1_sat1 = measured_sat1(size1 - 1, 0);
|
||||
|
||||
double t0_sat2 = measured_sat2(0, 0);
|
||||
int size2 = measured_sat2.col(0).n_rows;
|
||||
double t1_sat2 = measured_sat2(size2 - 1, 0);
|
||||
|
||||
double t0;
|
||||
double t1;
|
||||
if (t0_sat1 > t0_sat2)
|
||||
{
|
||||
t0 = t0_sat1;
|
||||
}
|
||||
else
|
||||
{
|
||||
t0 = t0_sat2;
|
||||
}
|
||||
|
||||
if (t1_sat1 > t1_sat2)
|
||||
{
|
||||
t1 = t1_sat2;
|
||||
}
|
||||
else
|
||||
{
|
||||
t1 = t1_sat1;
|
||||
}
|
||||
|
||||
arma::vec t = arma::linspace<arma::vec>(t0, t1, floor((t1 - t0) * 1e3));
|
||||
//conversion between arma::vec and std:vector
|
||||
arma::vec t_from_start = arma::linspace<arma::vec>(0, t1 - t0, floor((t1 - t0) * 1e3));
|
||||
@ -1037,6 +1076,15 @@ void HybridObservablesTest::check_results_duplicated_satellite(
|
||||
//compute error
|
||||
err_ch0_hz = meas_sat1_doppler_interp - meas_sat2_doppler_interp;
|
||||
|
||||
//save matlab file for further analysis
|
||||
std::vector<double> tmp_vector_common_time_s(t.colptr(0),
|
||||
t.colptr(0) + t.n_rows);
|
||||
|
||||
std::vector<double> tmp_vector_err_ch0_hz(err_ch0_hz.colptr(0),
|
||||
err_ch0_hz.colptr(0) + err_ch0_hz.n_rows);
|
||||
save_mat_xy(tmp_vector_common_time_s, tmp_vector_err_ch0_hz, std::string("measured_doppler_error_ch_" + std::to_string(ch_id)));
|
||||
|
||||
//compute statistics
|
||||
arma::vec err2_ch0 = arma::square(err_ch0_hz);
|
||||
double rmse_ch0 = sqrt(arma::mean(err2_ch0));
|
||||
|
||||
@ -1078,19 +1126,26 @@ void HybridObservablesTest::check_results_duplicated_satellite(
|
||||
}
|
||||
|
||||
//check results against the test tolerance
|
||||
ASSERT_LT(error_mean_ch0, 5);
|
||||
ASSERT_GT(error_mean_ch0, -5);
|
||||
EXPECT_LT(error_mean_ch0, 5);
|
||||
EXPECT_GT(error_mean_ch0, -5);
|
||||
//assuming PLL BW=35
|
||||
ASSERT_LT(error_var_ch0, 250);
|
||||
ASSERT_LT(max_error_ch0, 100);
|
||||
ASSERT_GT(min_error_ch0, -100);
|
||||
ASSERT_LT(rmse_ch0, 30);
|
||||
EXPECT_LT(error_var_ch0, 250);
|
||||
EXPECT_LT(max_error_ch0, 100);
|
||||
EXPECT_GT(min_error_ch0, -100);
|
||||
EXPECT_LT(rmse_ch0, 30);
|
||||
|
||||
//Carrier Phase error
|
||||
//2. RMSE
|
||||
arma::vec err_carrier_phase;
|
||||
|
||||
err_carrier_phase = delta_measured_carrier_phase_cycles;
|
||||
|
||||
//save matlab file for further analysis
|
||||
std::vector<double> tmp_vector_err_carrier_phase(err_carrier_phase.colptr(0),
|
||||
err_carrier_phase.colptr(0) + err_carrier_phase.n_rows);
|
||||
save_mat_xy(tmp_vector_common_time_s, tmp_vector_err_carrier_phase, std::string("measured_carrier_phase_error_ch_" + std::to_string(ch_id)));
|
||||
|
||||
|
||||
arma::vec err2_carrier_phase = arma::square(err_carrier_phase);
|
||||
double rmse_carrier_phase = sqrt(arma::mean(err2_carrier_phase));
|
||||
|
||||
@ -1132,18 +1187,24 @@ void HybridObservablesTest::check_results_duplicated_satellite(
|
||||
}
|
||||
|
||||
//check results against the test tolerance
|
||||
ASSERT_LT(rmse_carrier_phase, 0.25);
|
||||
ASSERT_LT(error_mean_carrier_phase, 0.2);
|
||||
ASSERT_GT(error_mean_carrier_phase, -0.2);
|
||||
ASSERT_LT(error_var_carrier_phase, 0.5);
|
||||
ASSERT_LT(max_error_carrier_phase, 0.5);
|
||||
ASSERT_GT(min_error_carrier_phase, -0.5);
|
||||
EXPECT_LT(rmse_carrier_phase, 0.25);
|
||||
EXPECT_LT(error_mean_carrier_phase, 0.2);
|
||||
EXPECT_GT(error_mean_carrier_phase, -0.2);
|
||||
EXPECT_LT(error_var_carrier_phase, 0.5);
|
||||
EXPECT_LT(max_error_carrier_phase, 0.5);
|
||||
EXPECT_GT(min_error_carrier_phase, -0.5);
|
||||
|
||||
//Pseudorange error
|
||||
//2. RMSE
|
||||
arma::vec err_pseudorange;
|
||||
|
||||
err_pseudorange = delta_measured_dist_m;
|
||||
|
||||
//save matlab file for further analysis
|
||||
std::vector<double> tmp_vector_err_pseudorange(err_pseudorange.colptr(0),
|
||||
err_pseudorange.colptr(0) + err_pseudorange.n_rows);
|
||||
save_mat_xy(tmp_vector_common_time_s, tmp_vector_err_pseudorange, std::string("measured_pr_error_ch_" + std::to_string(ch_id)));
|
||||
|
||||
arma::vec err2_pseudorange = arma::square(err_pseudorange);
|
||||
double rmse_pseudorange = sqrt(arma::mean(err2_pseudorange));
|
||||
|
||||
@ -1185,12 +1246,12 @@ void HybridObservablesTest::check_results_duplicated_satellite(
|
||||
}
|
||||
|
||||
//check results against the test tolerance
|
||||
ASSERT_LT(rmse_pseudorange, 3.0);
|
||||
ASSERT_LT(error_mean_pseudorange, 1.0);
|
||||
ASSERT_GT(error_mean_pseudorange, -1.0);
|
||||
ASSERT_LT(error_var_pseudorange, 10.0);
|
||||
ASSERT_LT(max_error_pseudorange, 10.0);
|
||||
ASSERT_GT(min_error_pseudorange, -10.0);
|
||||
EXPECT_LT(rmse_pseudorange, 3.0);
|
||||
EXPECT_LT(error_mean_pseudorange, 1.0);
|
||||
EXPECT_GT(error_mean_pseudorange, -1.0);
|
||||
EXPECT_LT(error_var_pseudorange, 10.0);
|
||||
EXPECT_LT(max_error_pseudorange, 10.0);
|
||||
EXPECT_GT(min_error_pseudorange, -10.0);
|
||||
}
|
||||
|
||||
bool HybridObservablesTest::save_mat_xy(std::vector<double>& x, std::vector<double>& y, std::string filename)
|
||||
@ -1499,7 +1560,9 @@ TEST_F(HybridObservablesTest, ValidationOfResults)
|
||||
FLAGS_DLL_bw_hz_start,
|
||||
FLAGS_PLL_narrow_bw_hz,
|
||||
FLAGS_DLL_narrow_bw_hz,
|
||||
FLAGS_extend_correlation_symbols);
|
||||
FLAGS_extend_correlation_symbols,
|
||||
FLAGS_smoother_length,
|
||||
FLAGS_high_dyn);
|
||||
|
||||
|
||||
for (unsigned int n = 0; n < gnss_synchro_vec.size(); n++)
|
||||
@ -1814,6 +1877,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults)
|
||||
check_results_duplicated_satellite(
|
||||
measured_obs_vec.at(sat1_ch_id),
|
||||
measured_obs_vec.at(sat2_ch_id),
|
||||
sat1_ch_id,
|
||||
"Duplicated sat [CH " + std::to_string(sat1_ch_id) + "," + std::to_string(sat2_ch_id) + "] PRNs " + std::to_string(gnss_synchro_vec.at(sat1_ch_id).PRN) + "," + std::to_string(gnss_synchro_vec.at(sat2_ch_id).PRN) + " ");
|
||||
}
|
||||
else
|
||||
@ -1883,6 +1947,18 @@ TEST_F(HybridObservablesTest, ValidationOfResults)
|
||||
measured_obs_vec.at(n).col(2).colptr(0) + measured_obs_vec.at(n).col(2).n_rows);
|
||||
save_mat_xy(tmp_vector_x4, tmp_vector_y4, std::string("measured_doppler_ch_" + std::to_string(n)));
|
||||
|
||||
std::vector<double> tmp_vector_x5(true_obs_vec.at(n).col(0).colptr(0),
|
||||
true_obs_vec.at(n).col(0).colptr(0) + true_obs_vec.at(n).col(0).n_rows);
|
||||
std::vector<double> tmp_vector_y5(true_obs_vec.at(n).col(3).colptr(0),
|
||||
true_obs_vec.at(n).col(3).colptr(0) + true_obs_vec.at(n).col(3).n_rows);
|
||||
save_mat_xy(tmp_vector_x5, tmp_vector_y5, std::string("true_cp_ch_" + std::to_string(n)));
|
||||
|
||||
std::vector<double> tmp_vector_x6(measured_obs_vec.at(n).col(0).colptr(0),
|
||||
measured_obs_vec.at(n).col(0).colptr(0) + measured_obs_vec.at(n).col(0).n_rows);
|
||||
std::vector<double> tmp_vector_y6(measured_obs_vec.at(n).col(3).colptr(0),
|
||||
measured_obs_vec.at(n).col(3).colptr(0) + measured_obs_vec.at(n).col(3).n_rows);
|
||||
save_mat_xy(tmp_vector_x6, tmp_vector_y6, std::string("measured_cp_ch_" + std::to_string(n)));
|
||||
|
||||
|
||||
if (epoch_counters_vec.at(n) > 10) //discard non-valid channels
|
||||
{
|
||||
|
||||
@ -38,6 +38,8 @@
|
||||
#include "rtklib_solver.h"
|
||||
#include "in_memory_configuration.h"
|
||||
#include "gnss_sdr_supl_client.h"
|
||||
#include "geofunctions.h"
|
||||
#include <armadillo>
|
||||
|
||||
|
||||
rtk_t configure_rtklib_options()
|
||||
@ -366,10 +368,6 @@ TEST(RTKLibSolverTest, test1)
|
||||
//
|
||||
// gnss_synchro_map[0] = tmp_obs;
|
||||
// gnss_synchro_map[0].PRN = 1;
|
||||
// gnss_synchro_map[0].RX_time = 518449.000000;
|
||||
// gnss_synchro_map[0].Pseudorange_m = 22816591.664859;
|
||||
// gnss_synchro_map[0].Carrier_Doppler_hz = -2579.334343;
|
||||
// gnss_synchro_map[0].Carrier_phase_rads = 794858.014183;
|
||||
|
||||
//load from xml (boost serialize)
|
||||
std::string file_name = path + "data/rtklib_test/obs_test1.xml";
|
||||
@ -417,10 +415,6 @@ TEST(RTKLibSolverTest, test1)
|
||||
// p_time += boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6));
|
||||
// std::cout << TEXT_MAGENTA << "Observable RX time (GPST) " << boost::posix_time::to_simple_string(p_time) << TEXT_RESET << std::endl;
|
||||
|
||||
std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
|
||||
<< " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude()
|
||||
<< " [deg], Height = " << d_ls_pvt->get_height() << " [m]" << std::endl;
|
||||
|
||||
std::cout << "RTKLIB Position at RX TOW = " << gnss_synchro_map.begin()->second.RX_time
|
||||
<< " in ECEF (X,Y,Z,t[meters]) = " << std::fixed << std::setprecision(16)
|
||||
<< d_ls_pvt->pvt_sol.rr[0] << ","
|
||||
@ -433,8 +427,32 @@ TEST(RTKLibSolverTest, test1)
|
||||
|
||||
//todo: check here the positioning error against the reference position generated with gnss-sim
|
||||
//reference position on in WGS84: Lat (deg), Long (deg) , H (m): 30.286502,120.032669,100
|
||||
arma::vec LLH = {30.286502, 120.032669, 100}; //ref position for this scenario
|
||||
|
||||
double error_LLH_m = great_circle_distance(LLH(0), LLH(1), d_ls_pvt->get_latitude(), d_ls_pvt->get_longitude());
|
||||
std::cout << "Lat, Long, H error: " << d_ls_pvt->get_latitude() - LLH(0)
|
||||
<< "," << d_ls_pvt->get_longitude() - LLH(1)
|
||||
<< "," << d_ls_pvt->get_height() - LLH(2) << " [deg,deg,meters]" << std::endl;
|
||||
|
||||
std::cout << "Haversine Great Circle error LLH distance: " << error_LLH_m << " [meters]" << std::endl;
|
||||
|
||||
arma::vec v_eb_n = {0.0, 0.0, 0.0};
|
||||
arma::vec true_r_eb_e;
|
||||
arma::vec true_v_eb_e;
|
||||
pv_Geo_to_ECEF(degtorad(LLH(0)), degtorad(LLH(1)), LLH(2), v_eb_n, true_r_eb_e, true_v_eb_e);
|
||||
|
||||
arma::vec measured_r_eb_e = {d_ls_pvt->pvt_sol.rr[0], d_ls_pvt->pvt_sol.rr[1], d_ls_pvt->pvt_sol.rr[2]};
|
||||
|
||||
arma::vec error_r_eb_e = measured_r_eb_e - true_r_eb_e;
|
||||
|
||||
std::cout << "ECEF position error vector: " << error_r_eb_e << " [meters]" << std::endl;
|
||||
|
||||
double error_3d_m = arma::norm(error_r_eb_e, 2);
|
||||
|
||||
std::cout << "3D positioning error: " << error_3d_m << " [meters]" << std::endl;
|
||||
|
||||
//check results against the test tolerance
|
||||
ASSERT_LT(error_3d_m, 0.2);
|
||||
pvt_valid = true;
|
||||
}
|
||||
}
|
||||
|
||||
@ -17,3 +17,7 @@
|
||||
#
|
||||
|
||||
add_subdirectory(front-end-cal)
|
||||
|
||||
if(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA)
|
||||
add_subdirectory(rinex2assist)
|
||||
endif(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA)
|
||||
|
||||
@ -33,7 +33,7 @@ function [GNSS_tracking] = dll_pll_veml_read_tracking_dump (filename, count)
|
||||
|
||||
m = nargchk (1,2,nargin);
|
||||
|
||||
num_float_vars = 17;
|
||||
num_float_vars = 19;
|
||||
num_unsigned_long_int_vars = 1;
|
||||
num_double_vars = 1;
|
||||
num_unsigned_int_vars = 1;
|
||||
@ -114,17 +114,23 @@ else
|
||||
fseek(f,bytes_shift,'bof'); % move to next float
|
||||
v16 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
|
||||
bytes_shift = bytes_shift + float_size_bytes;
|
||||
fseek(f,bytes_shift,'bof'); % move to next interleaved float
|
||||
fseek(f,bytes_shift,'bof'); % move to next float
|
||||
v17 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
|
||||
bytes_shift = bytes_shift + float_size_bytes;
|
||||
fseek(f,bytes_shift,'bof'); % move to next float
|
||||
v18 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes);
|
||||
v18 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
|
||||
bytes_shift = bytes_shift + float_size_bytes;
|
||||
fseek(f,bytes_shift,'bof'); % move to next interleaved float
|
||||
v19 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
|
||||
bytes_shift = bytes_shift + float_size_bytes;
|
||||
fseek(f,bytes_shift,'bof'); % move to next float
|
||||
v20 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes);
|
||||
bytes_shift = bytes_shift + float_size_bytes;
|
||||
fseek(f,bytes_shift,'bof'); % move to next double
|
||||
v19 = fread (f, count, 'double', skip_bytes_each_read - double_size_bytes);
|
||||
v21 = fread (f, count, 'double', skip_bytes_each_read - double_size_bytes);
|
||||
bytes_shift = bytes_shift + double_size_bytes;
|
||||
fseek(f,bytes_shift,'bof'); % move to next unsigned int
|
||||
v20 = fread (f, count, 'uint', skip_bytes_each_read - unsigned_int_size_bytes);
|
||||
v22 = fread (f, count, 'uint', skip_bytes_each_read - unsigned_int_size_bytes);
|
||||
fclose (f);
|
||||
|
||||
GNSS_tracking.VE = v1;
|
||||
@ -137,15 +143,17 @@ else
|
||||
GNSS_tracking.PRN_start_sample = v8;
|
||||
GNSS_tracking.acc_carrier_phase_rad = v9;
|
||||
GNSS_tracking.carrier_doppler_hz = v10;
|
||||
GNSS_tracking.code_freq_hz = v11;
|
||||
GNSS_tracking.carr_error = v12;
|
||||
GNSS_tracking.carr_nco = v13;
|
||||
GNSS_tracking.code_error = v14;
|
||||
GNSS_tracking.code_nco = v15;
|
||||
GNSS_tracking.CN0_SNV_dB_Hz = v16;
|
||||
GNSS_tracking.carrier_lock_test = v17;
|
||||
GNSS_tracking.var1 = v18;
|
||||
GNSS_tracking.var2 = v19;
|
||||
GNSS_tracking.PRN = v20;
|
||||
GNSS_tracking.carrier_doppler_rate_hz_s = v11;
|
||||
GNSS_tracking.code_freq_hz = v12;
|
||||
GNSS_tracking.code_freq_rate_hz_s = v13;
|
||||
GNSS_tracking.carr_error = v14;
|
||||
GNSS_tracking.carr_nco = v15;
|
||||
GNSS_tracking.code_error = v16;
|
||||
GNSS_tracking.code_nco = v17;
|
||||
GNSS_tracking.CN0_SNV_dB_Hz = v18;
|
||||
GNSS_tracking.carrier_lock_test = v19;
|
||||
GNSS_tracking.var1 = v20;
|
||||
GNSS_tracking.var2 = v21;
|
||||
GNSS_tracking.PRN = v22;
|
||||
end
|
||||
|
||||
|
||||
56
src/utils/rinex2assist/CMakeLists.txt
Normal file
56
src/utils/rinex2assist/CMakeLists.txt
Normal file
@ -0,0 +1,56 @@
|
||||
# Copyright (C) 2012-2018 (see AUTHORS file for a list of contributors)
|
||||
#
|
||||
# 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 <https://www.gnu.org/licenses/>.
|
||||
#
|
||||
|
||||
find_package(GPSTK QUIET)
|
||||
if(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK)
|
||||
set(GPSTK_LIBRARY ${CMAKE_CURRENT_SOURCE_DIR}/../../../thirdparty/gpstk-${GNSSSDR_GPSTK_LOCAL_VERSION}/install/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gpstk${CMAKE_SHARED_LIBRARY_SUFFIX} )
|
||||
set(GPSTK_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../thirdparty/gpstk-${GNSSSDR_GPSTK_LOCAL_VERSION}/install/include )
|
||||
endif(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK)
|
||||
|
||||
set(CMAKE_INCLUDE_PATH ${CMAKE_INCLUDE_PATH} ${GPSTK_INCLUDE_DIR}/gpstk)
|
||||
|
||||
include_directories(
|
||||
${CMAKE_SOURCE_DIR}/src/core/system_parameters
|
||||
${GFlags_INCLUDE_DIRS}
|
||||
${Boost_INCLUDE_DIRS}
|
||||
${GPSTK_INCLUDE_DIR}/gpstk
|
||||
${GPSTK_INCLUDE_DIR}
|
||||
)
|
||||
|
||||
add_executable(rinex2assist ${CMAKE_CURRENT_SOURCE_DIR}/main.cc)
|
||||
|
||||
target_link_libraries(rinex2assist
|
||||
${Boost_LIBRARIES}
|
||||
${GPSTK_LIBRARY}
|
||||
${GFlags_LIBS}
|
||||
gnss_sp_libs
|
||||
gnss_rx)
|
||||
|
||||
if(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK)
|
||||
add_dependencies(rinex2assist gpstk-${GNSSSDR_GPSTK_LOCAL_VERSION})
|
||||
endif(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK)
|
||||
|
||||
|
||||
add_custom_command(TARGET rinex2assist POST_BUILD
|
||||
COMMAND ${CMAKE_COMMAND} -E copy $<TARGET_FILE:rinex2assist>
|
||||
${CMAKE_SOURCE_DIR}/install/$<TARGET_FILE_NAME:rinex2assist>)
|
||||
|
||||
install(TARGETS rinex2assist
|
||||
RUNTIME DESTINATION bin
|
||||
COMPONENT "rinex2assist"
|
||||
)
|
||||
228
src/utils/rinex2assist/main.cc
Normal file
228
src/utils/rinex2assist/main.cc
Normal file
@ -0,0 +1,228 @@
|
||||
/*!
|
||||
* \file main.cc
|
||||
* \brief converts navigation RINEX files into XML files for Assisted GNSS.
|
||||
* \author Carles Fernandez-Prades, 2018. cfernandez(at)cttc.cat
|
||||
*
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (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 <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#include "gps_ephemeris.h"
|
||||
#include "galileo_ephemeris.h"
|
||||
#include <gflags/gflags.h>
|
||||
#include <gpstk/Rinex3NavHeader.hpp>
|
||||
#include <gpstk/Rinex3NavData.hpp>
|
||||
#include <gpstk/Rinex3NavStream.hpp>
|
||||
#include <boost/archive/xml_oarchive.hpp>
|
||||
#include <boost/serialization/map.hpp>
|
||||
#include <iostream>
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
const std::string intro_help(
|
||||
std::string("\n rinex2assist converts navigation RINEX files into XML files for Assisted GNSS\n") +
|
||||
"Copyright (C) 2018 (see AUTHORS file for a list of contributors)\n" +
|
||||
"This program comes with ABSOLUTELY NO WARRANTY;\n" +
|
||||
"See COPYING file to see a copy of the General Public License.\n \n" +
|
||||
"Usage: \n" +
|
||||
" rinex2assist <RINEX Nav file input> [<XML file output>]");
|
||||
|
||||
google::SetUsageMessage(intro_help);
|
||||
google::SetVersionString("1.0");
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
if ((argc < 2) or (argc > 3))
|
||||
{
|
||||
std::cerr << "Usage:" << std::endl;
|
||||
std::cerr << " " << argv[0]
|
||||
<< " <RINEX Nav file input> [<XML file output>]"
|
||||
<< std::endl;
|
||||
google::ShutDownCommandLineFlags();
|
||||
return 1;
|
||||
}
|
||||
std::string xml_filename;
|
||||
if (argc == 3)
|
||||
{
|
||||
xml_filename = argv[2];
|
||||
}
|
||||
|
||||
std::map<int, Gps_Ephemeris> eph_map;
|
||||
std::map<int, Galileo_Ephemeris> eph_gal_map;
|
||||
|
||||
int i = 0;
|
||||
int j = 0;
|
||||
try
|
||||
{
|
||||
// Read nav file
|
||||
gpstk::Rinex3NavStream rnffs(argv[1]); // Open navigation data file
|
||||
gpstk::Rinex3NavData rne;
|
||||
gpstk::Rinex3NavHeader hdr;
|
||||
|
||||
// Read header
|
||||
rnffs >> hdr;
|
||||
|
||||
// Check that it really is a RINEX navigation file
|
||||
if (hdr.fileType.substr(0, 1).compare("N") != 0)
|
||||
{
|
||||
std::cerr << "This is not a valid RINEX navigation file, or file not found." << std::endl;
|
||||
std::cerr << "No XML file will be created." << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Read navigation data
|
||||
while (rnffs >> rne)
|
||||
{
|
||||
if (rne.satSys.compare("G") == 0 or rne.satSys.empty())
|
||||
{
|
||||
// Fill GPS ephemeris object
|
||||
Gps_Ephemeris eph;
|
||||
eph.i_satellite_PRN = rne.PRNID;
|
||||
eph.d_TOW = rne.xmitTime;
|
||||
eph.d_IODE_SF2 = rne.IODE;
|
||||
eph.d_IODE_SF3 = rne.IODE;
|
||||
eph.d_Crs = rne.Crs;
|
||||
eph.d_Delta_n = rne.dn;
|
||||
eph.d_M_0 = rne.M0;
|
||||
eph.d_Cuc = rne.Cuc;
|
||||
eph.d_e_eccentricity = rne.ecc;
|
||||
eph.d_Cus = rne.Cus;
|
||||
eph.d_sqrt_A = rne.Ahalf;
|
||||
eph.d_Toe = rne.Toe;
|
||||
eph.d_Toc = rne.Toc;
|
||||
eph.d_Cic = rne.Cic;
|
||||
eph.d_OMEGA0 = rne.OMEGA0;
|
||||
eph.d_Cis = rne.Cis;
|
||||
eph.d_i_0 = rne.i0;
|
||||
eph.d_Crc = rne.Crc;
|
||||
eph.d_OMEGA = rne.w;
|
||||
eph.d_OMEGA_DOT = rne.OMEGAdot;
|
||||
eph.d_IDOT = rne.idot;
|
||||
eph.i_code_on_L2 = rne.codeflgs; //
|
||||
eph.i_GPS_week = rne.weeknum;
|
||||
eph.b_L2_P_data_flag = rne.L2Pdata;
|
||||
eph.i_SV_accuracy = rne.accuracy;
|
||||
eph.i_SV_health = rne.health;
|
||||
eph.d_TGD = rne.Tgd;
|
||||
eph.d_IODC = rne.IODC;
|
||||
eph.i_AODO = 0; //
|
||||
eph.b_fit_interval_flag = (rne.fitint > 4) ? 1 : 0;
|
||||
eph.d_spare1 = 0.0;
|
||||
eph.d_spare2 = 0.0;
|
||||
eph.d_A_f0 = rne.af0;
|
||||
eph.d_A_f1 = rne.af1;
|
||||
eph.d_A_f2 = rne.af2;
|
||||
eph.b_integrity_status_flag = 0; //
|
||||
eph.b_alert_flag = 0; //
|
||||
eph.b_antispoofing_flag = 0; //
|
||||
eph_map[i] = eph;
|
||||
i++;
|
||||
}
|
||||
if (rne.satSys.compare("E") == 0)
|
||||
{
|
||||
// Fill Galileo ephemeris object
|
||||
Galileo_Ephemeris eph;
|
||||
eph.i_satellite_PRN = rne.PRNID;
|
||||
eph.M0_1 = rne.M0;
|
||||
eph.e_1 = rne.ecc;
|
||||
eph.A_1 = rne.Ahalf;
|
||||
eph.OMEGA_0_2 = rne.OMEGA0;
|
||||
eph.i_0_2 = rne.i0;
|
||||
eph.omega_2 = rne.w;
|
||||
eph.OMEGA_dot_3 = rne.OMEGAdot;
|
||||
eph.iDot_2 = rne.idot;
|
||||
eph.C_uc_3 = rne.Cuc;
|
||||
eph.C_us_3 = rne.Cus;
|
||||
eph.C_rc_3 = rne.Crc;
|
||||
eph.C_rs_3 = rne.Crs;
|
||||
eph.C_ic_4 = rne.Cic;
|
||||
eph.C_is_4 = rne.Cis;
|
||||
eph.t0e_1 = rne.Toe;
|
||||
eph.t0c_4 = rne.Toc;
|
||||
eph.af0_4 = rne.af0;
|
||||
eph.af1_4 = rne.af1;
|
||||
eph.af2_4 = rne.af2;
|
||||
eph_gal_map[j] = eph;
|
||||
j++;
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (std::exception& e)
|
||||
{
|
||||
std::cerr << "Error reading the RINEX file: " << e.what() << std::endl;
|
||||
std::cerr << "No XML file will be created." << std::endl;
|
||||
google::ShutDownCommandLineFlags();
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (i == 0 and j == 0)
|
||||
{
|
||||
std::cerr << "No navigation data found in the RINEX file. No XML file will be created." << std::endl;
|
||||
google::ShutDownCommandLineFlags();
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Write XML
|
||||
if (i != 0)
|
||||
{
|
||||
std::ofstream ofs;
|
||||
if (xml_filename.empty())
|
||||
{
|
||||
xml_filename = "eph_GPS_L1CA.xml";
|
||||
}
|
||||
try
|
||||
{
|
||||
ofs.open(xml_filename.c_str(), std::ofstream::trunc | std::ofstream::out);
|
||||
boost::archive::xml_oarchive xml(ofs);
|
||||
xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", eph_map);
|
||||
}
|
||||
catch (std::exception& e)
|
||||
{
|
||||
std::cerr << "Problem creating the XML file: " << e.what() << std::endl;
|
||||
google::ShutDownCommandLineFlags();
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
if (j != 0)
|
||||
{
|
||||
std::ofstream ofs2;
|
||||
xml_filename = "eph_Galileo_E1.xml";
|
||||
try
|
||||
{
|
||||
ofs2.open(xml_filename.c_str(), std::ofstream::trunc | std::ofstream::out);
|
||||
boost::archive::xml_oarchive xml(ofs2);
|
||||
xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", eph_gal_map);
|
||||
}
|
||||
catch (std::exception& e)
|
||||
{
|
||||
std::cerr << "Problem creating the XML file: " << e.what() << std::endl;
|
||||
google::ShutDownCommandLineFlags();
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
google::ShutDownCommandLineFlags();
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user