mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-12 19:20:32 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into gnsstk

Browse Source
This commit is contained in:
Carles Fernandez 2022-07-04 17:10:52 +02:00
commit dc027e95ff
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
2 changed files with 53 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.cc
View File

@ -844,18 +844,17 @@ void kf_vtl_tracking::init_kf(double acq_code_phase_chips, double acq_doppler_hz
const double Ti = d_correlation_length_ms * 0.001;
// state vector: code_phase_chips, carrier_phase_rads, carrier_freq_hz,carrier_freq_rate_hz, code_freq_chips_s
d_F = arma::mat(5, 5);
d_F << 1 << 0 << 0 << 0 << Ti << arma::endr
<< 0 << 1 << 2.0 * GNSS_PI * Ti << GNSS_PI * (Ti * Ti) << 0 << arma::endr
<< 0 << 0 << 1 << Ti << 0 << arma::endr
<< 0 << 0 << 0 << 1 << 0 << arma::endr
<< 0 << 0 << 0 << 0 << 1 << arma::endr;
d_F = {{1, 0, 0, 0, Ti},
{0, 1, 2.0 * GNSS_PI * Ti, GNSS_PI * (Ti * Ti), 0},
{0, 0, 1, Ti, 0},
{0, 0, 0, 1, 0},
{0, 0, 0, 0, 1}};
const double B = d_code_chip_rate / d_signal_carrier_freq; // carrier to code rate factor
d_H = arma::mat(2, 5);
d_H << 1 << 0 << -B * Ti / 2.0 << B * (Ti * Ti) / 6.0 << 0 << arma::endr
<< 0 << 1 << -GNSS_PI * Ti << GNSS_PI * (Ti * Ti) / 3.0 << 0 << arma::endr;
d_H = {{1, 0, -B * Ti / 2.0, B * (Ti * Ti) / 6.0, 0},
{0, 1, -GNSS_PI * Ti, GNSS_PI * (Ti * Ti) / 3.0, 0}};
// Phase noise variance
// const double CN0_lin = pow(10.0, d_trk_parameters.expected_cn0_dbhz / 10.0); // CN0 in Hz
// const double N_periods = 1; // Only 1 interval
@ -866,32 +865,26 @@ void kf_vtl_tracking::init_kf(double acq_code_phase_chips, double acq_doppler_hz
d_R = arma::mat(2, 2);
// d_R << Sigma2_Tau << 0 << arma::endr
// << 0 << Sigma2_Phase << arma::endr;
d_R << pow(d_trk_parameters.code_disc_sd_chips, 2.0) << 0 << arma::endr
<< 0 << pow(d_trk_parameters.carrier_disc_sd_rads, 2.0) << arma::endr;
d_R = {{pow(d_trk_parameters.code_disc_sd_chips, 2.0), 0},
{0, pow(d_trk_parameters.carrier_disc_sd_rads, 2.0)}};
// system covariance matrix (static)
d_Q = arma::mat(5, 5);
d_Q << pow(d_trk_parameters.code_phase_sd_chips, 2.0) << 0 << 0 << 0 << 0 << arma::endr
<< 0 << pow(d_trk_parameters.carrier_phase_sd_rad, 2.0) << 0 << 0 << 0 << arma::endr
<< 0 << 0 << pow(d_trk_parameters.carrier_freq_sd_hz, 2.0) << 0 << 0 << arma::endr
<< 0 << 0 << 0 << pow(d_trk_parameters.carrier_freq_rate_sd_hz_s, 2.0) << 0 << arma::endr
<< 0 << 0 << 0 << 0 << pow(d_trk_parameters.code_rate_sd_chips_s, 2.0) << arma::endr;
d_Q = {{pow(d_trk_parameters.code_phase_sd_chips, 2.0), 0, 0, 0, 0},
{0, pow(d_trk_parameters.carrier_phase_sd_rad, 2.0), 0, 0, 0},
{0, 0, pow(d_trk_parameters.carrier_freq_sd_hz, 2.0), 0, 0},
{0, 0, 0, pow(d_trk_parameters.carrier_freq_rate_sd_hz_s, 2.0), 0},
{0, 0, 0, 0, pow(d_trk_parameters.code_rate_sd_chips_s, 2.0)}};
// initial Kalman covariance matrix
d_P_old_old = arma::mat(5, 5);
d_P_old_old << pow(d_trk_parameters.init_code_phase_sd_chips, 2.0) << 0 << 0 << 0 << 0 << arma::endr
<< 0 << pow(d_trk_parameters.init_carrier_phase_sd_rad, 2.0) << 0 << 0 << 0 << arma::endr
<< 0 << 0 << pow(d_trk_parameters.init_carrier_freq_sd_hz, 2.0) << 0 << 0 << arma::endr
<< 0 << 0 << 0 << pow(d_trk_parameters.init_carrier_freq_rate_sd_hz_s, 2.0) << 0 << arma::endr
<< 0 << 0 << 0 << 0 << pow(d_trk_parameters.init_code_rate_sd_chips_s, 2.0) << arma::endr;
d_P_old_old = {{pow(d_trk_parameters.init_code_phase_sd_chips, 2.0), 0, 0, 0, 0},
{0, pow(d_trk_parameters.init_carrier_phase_sd_rad, 2.0), 0, 0, 0},
{0, 0, pow(d_trk_parameters.init_carrier_freq_rate_sd_hz_s, 2.0), 0, 0},
{0, 0, 0, pow(d_trk_parameters.init_carrier_freq_rate_sd_hz_s, 2.0), 0},
{0, 0, 0, 0, pow(d_trk_parameters.init_code_rate_sd_chips_s, 2.0)}};
// init state vector
d_x_old_old = arma::vec(5);
// states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s, code_freq_rate_chips_s
d_x_old_old << acq_code_phase_chips << 0 << acq_doppler_hz << 0 << 0 << arma::endr;
d_x_old_old = {acq_code_phase_chips, 0, acq_doppler_hz, 0, 0};
// std::cout << "F: " << d_F << "\n";
// std::cout << "H: " << d_H << "\n";
// std::cout << "R: " << d_R << "\n";
@ -907,27 +900,26 @@ void kf_vtl_tracking::update_kf_narrow_integration_time()
const double Ti = d_current_correlation_time_s;
// state vector: code_phase_chips, carrier_phase_rads, carrier_freq_hz,carrier_freq_rate_hz, code_freq_chips_s
d_F << 1 << 0 << 0 << 0 << Ti << arma::endr
<< 0 << 1 << 2.0 * GNSS_PI * Ti << GNSS_PI * (Ti * Ti) << 0 << arma::endr
<< 0 << 0 << 1 << Ti << 0 << arma::endr
<< 0 << 0 << 0 << 1 << 0 << arma::endr
<< 0 << 0 << 0 << 0 << 1 << arma::endr;
d_F = {{1, 0, 0, 0, Ti},
{0, 1, 2.0 * GNSS_PI * Ti, GNSS_PI * (Ti * Ti), 0},
{0, 0, 1, Ti, 0},
{0, 0, 0, 1, 0},
{0, 0, 0, 0, 1}};
const double B = d_code_chip_rate / d_signal_carrier_freq; // carrier to code rate factor
d_H << 1 << 0 << -B * Ti / 2.0 << B * (Ti * Ti) / 6.0 << 0 << arma::endr
<< 0 << 1 << -GNSS_PI * Ti << GNSS_PI * (Ti * Ti) / 3.0 << 0 << arma::endr;
d_H = {{1, 0, -B * Ti / 2.0, B * (Ti * Ti) / 6.0, 0},
{0, 1, -GNSS_PI * Ti, GNSS_PI * (Ti * Ti) / 3.0, 0}};
// measurement covariance matrix (static)
d_R << pow(d_trk_parameters.code_disc_sd_chips, 2.0) << 0 << arma::endr
<< 0 << pow(d_trk_parameters.carrier_disc_sd_rads, 2.0) << arma::endr;
d_R = {{pow(d_trk_parameters.code_disc_sd_chips, 2.0), 0},
{0, pow(d_trk_parameters.carrier_disc_sd_rads, 2.0)}};
// system covariance matrix (static)
d_Q << pow(d_trk_parameters.narrow_code_phase_sd_chips, 2.0) << 0 << 0 << 0 << 0 << arma::endr
<< 0 << pow(d_trk_parameters.narrow_carrier_phase_sd_rad, 2.0) << 0 << 0 << 0 << arma::endr
<< 0 << 0 << pow(d_trk_parameters.narrow_carrier_freq_sd_hz, 2.0) << 0 << 0 << arma::endr
<< 0 << 0 << 0 << pow(d_trk_parameters.narrow_carrier_freq_rate_sd_hz_s, 2.0) << 0 << arma::endr
<< 0 << 0 << 0 << 0 << pow(d_trk_parameters.narrow_code_rate_sd_chips_s, 2.0) << arma::endr;
d_Q = {{pow(d_trk_parameters.narrow_code_phase_sd_chips, 2.0), 0, 0, 0, 0},
{0, pow(d_trk_parameters.narrow_carrier_phase_sd_rad, 2.0), 0, 0, 0},
{0, 0, pow(d_trk_parameters.narrow_carrier_freq_sd_hz, 2.0), 0, 0},
{0, 0, 0, pow(d_trk_parameters.narrow_carrier_freq_rate_sd_hz_s, 2.0), 0},
{0, 0, 0, 0, pow(d_trk_parameters.narrow_code_rate_sd_chips_s, 2.0)}};
}
@ -938,8 +930,8 @@ void kf_vtl_tracking::update_kf_cn0(double current_cn0_dbhz)
const double B = d_code_chip_rate / d_signal_carrier_freq; // carrier to code rate factor
d_H = arma::mat(2, 5);
d_H << 1 << 0 << -B * Ti / 2.0 << B * (Ti * Ti) / 6.0 << 0 << arma::endr
<< 0 << 1 << -GNSS_PI * Ti << GNSS_PI * (Ti * Ti) / 3.0 << 0 << arma::endr;
d_H = {{1, 0, -B * Ti / 2.0, B * (Ti * Ti) / 6.0, 0},
{0, 1, -GNSS_PI * Ti, GNSS_PI * (Ti * Ti) / 3.0, 0}};
// Phase noise variance
const double CN0_lin = pow(10.0, current_cn0_dbhz / 10.0); // CN0 in Hz
@ -949,8 +941,8 @@ void kf_vtl_tracking::update_kf_cn0(double current_cn0_dbhz)
// measurement covariance matrix (static)
d_R = arma::mat(2, 2);
d_R << Sigma2_Tau << 0 << arma::endr
<< 0 << Sigma2_Phase << arma::endr;
d_R = {{Sigma2_Tau, 0},
{0, Sigma2_Phase}};
}

24
src/tests/CMakeLists.txt
View File

@ -43,6 +43,11 @@ if(NOT GOOGLETEST_FOUND)
else()
set(GOOGLETEST_GIT_TAG release-${GNSSSDR_GTEST_LOCAL_VERSION})
endif()
if(GNSSSDR_GTEST_LOCAL_VERSION VERSION_LESS 1.12.0)
set(DEBUG_DECORATION "d")
else()
set(DEBUG_DECORATION "")
endif()
if(CMAKE_VERSION VERSION_LESS 3.2)
ExternalProject_Add(gtest-${GNSSSDR_GTEST_LOCAL_VERSION}
GIT_REPOSITORY https://github.com/google/googletest
@ -65,11 +70,12 @@ if(NOT GOOGLETEST_FOUND)
${CMAKE_BINARY_DIR}/gtest-${GNSSSDR_GTEST_LOCAL_VERSION}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtest${CMAKE_STATIC_LIBRARY_SUFFIX}
${CMAKE_BINARY_DIR}/gtest-${GNSSSDR_GTEST_LOCAL_VERSION}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main${CMAKE_STATIC_LIBRARY_SUFFIX}
)
if((CMAKE_BUILD_TYPE STREQUAL Debug) OR (CMAKE_BUILD_TYPE STREQUAL NoOptWithASM) OR
(CMAKE_BUILD_TYPE STREQUAL Coverage) OR (CMAKE_BUILD_TYPE STREQUAL ASAN)) # Workaround for Ninja generator
set(GOOGLETEST_BUILD_BYPRODUCTS
#${CMAKE_BINARY_DIR}/gtest-${GNSSSDR_GTEST_LOCAL_VERSION}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtestd${CMAKE_STATIC_LIBRARY_SUFFIX}
#${CMAKE_BINARY_DIR}/gtest-${GNSSSDR_GTEST_LOCAL_VERSION}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_maind${CMAKE_STATIC_LIBRARY_SUFFIX}
${CMAKE_BINARY_DIR}/gtest-${GNSSSDR_GTEST_LOCAL_VERSION}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtest${DEBUG_DECORATION}${CMAKE_STATIC_LIBRARY_SUFFIX}
${CMAKE_BINARY_DIR}/gtest-${GNSSSDR_GTEST_LOCAL_VERSION}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main${DEBUG_DECORATION}${CMAKE_STATIC_LIBRARY_SUFFIX}
)
endif()
ExternalProject_Add(gtest-${GNSSSDR_GTEST_LOCAL_VERSION}
@ -110,15 +116,15 @@ if(NOT GOOGLETEST_FOUND)
IMPORTED_LOCATION_RELWITHDEBINFO ${binary_dir}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtest${CMAKE_STATIC_LIBRARY_SUFFIX}
IMPORTED_LOCATION_MINSIZEREL ${binary_dir}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtest${CMAKE_STATIC_LIBRARY_SUFFIX}
INTERFACE_INCLUDE_DIRECTORIES ${CMAKE_BINARY_DIR}/thirdparty/gtest/gtest-${GNSSSDR_GTEST_LOCAL_VERSION}/googletest/include
INTERFACE_LINK_LIBRARIES ${binary_dir}/lib/gtest${CMAKE_STATIC_LIBRARY_SUFFIX}
INTERFACE_LINK_LIBRARIES ${binary_dir}/lib/gtest$<$<CONFIG:Debug>:${DEBUG_DECORATION}>${CMAKE_STATIC_LIBRARY_SUFFIX}
)
if((CMAKE_GENERATOR STREQUAL Xcode) OR MSVC)
set_target_properties(GTest::GTest PROPERTIES
IMPORTED_LOCATION_DEBUG ${binary_dir}/lib/Debug/${CMAKE_FIND_LIBRARY_PREFIXES}gtest${CMAKE_STATIC_LIBRARY_SUFFIX}
IMPORTED_LOCATION_DEBUG ${binary_dir}/lib/Debug/${CMAKE_FIND_LIBRARY_PREFIXES}gtest${DEBUG_DECORATION}${CMAKE_STATIC_LIBRARY_SUFFIX}
IMPORTED_LOCATION_RELEASE ${binary_dir}/lib/Release/${CMAKE_FIND_LIBRARY_PREFIXES}gtest${CMAKE_STATIC_LIBRARY_SUFFIX}
IMPORTED_LOCATION_RELWITHDEBINFO ${binary_dir}/lib/RelWithDebInfo/${CMAKE_FIND_LIBRARY_PREFIXES}gtest${CMAKE_STATIC_LIBRARY_SUFFIX}
IMPORTED_LOCATION_MINSIZEREL ${binary_dir}/lib/MinSizeRel/${CMAKE_FIND_LIBRARY_PREFIXES}gtest${CMAKE_STATIC_LIBRARY_SUFFIX}
INTERFACE_LINK_LIBRARIES ${binary_dir}/lib/$<$<CONFIG:Debug>:Debug/>$<$<CONFIG:Release>:Release/>$<$<CONFIG:RelWithDebInfo>:RelWithDebInfo/>$<$<CONFIG:MinSizeRel>:MinSizeRel/>gtest${CMAKE_STATIC_LIBRARY_SUFFIX}
INTERFACE_LINK_LIBRARIES ${binary_dir}/lib/$<$<CONFIG:Debug>:Debug/>$<$<CONFIG:Release>:Release/>$<$<CONFIG:RelWithDebInfo>:RelWithDebInfo/>$<$<CONFIG:MinSizeRel>:MinSizeRel/>gtest$<$<CONFIG:Debug>:${DEBUG_DECORATION}>${CMAKE_STATIC_LIBRARY_SUFFIX}
)
endif()
set_target_properties(GTest::GTest PROPERTIES
@ -137,20 +143,20 @@ if(NOT GOOGLETEST_FOUND)
MAP_IMPORTED_CONFIG_O3WITHASM RelWithDebInfo
MAP_IMPORTED_CONFIG_ASAN Debug
IMPORTED_LOCATION_NONE ${binary_dir}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main${CMAKE_STATIC_LIBRARY_SUFFIX}
IMPORTED_LOCATION_DEBUG ${binary_dir}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main${CMAKE_STATIC_LIBRARY_SUFFIX}
IMPORTED_LOCATION_DEBUG ${binary_dir}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main${DEBUG_DECORATION}${CMAKE_STATIC_LIBRARY_SUFFIX}
IMPORTED_LOCATION_RELEASE ${binary_dir}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main${CMAKE_STATIC_LIBRARY_SUFFIX}
IMPORTED_LOCATION_RELWITHDEBINFO ${binary_dir}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main${CMAKE_STATIC_LIBRARY_SUFFIX}
IMPORTED_LOCATION_MINSIZEREL ${binary_dir}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main${CMAKE_STATIC_LIBRARY_SUFFIX}
INTERFACE_INCLUDE_DIRECTORIES ${CMAKE_BINARY_DIR}/thirdparty/gtest/gtest-${GNSSSDR_GTEST_LOCAL_VERSION}/googletest/include
INTERFACE_LINK_LIBRARIES ${binary_dir}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main${CMAKE_STATIC_LIBRARY_SUFFIX}
INTERFACE_LINK_LIBRARIES ${binary_dir}/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main$<$<CONFIG:Debug>:${DEBUG_DECORATION}>${CMAKE_STATIC_LIBRARY_SUFFIX}
)
if((CMAKE_GENERATOR STREQUAL Xcode) OR MSVC)
set_target_properties(GTest::Main PROPERTIES
IMPORTED_LOCATION_DEBUG ${binary_dir}/lib/Debug/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main${CMAKE_STATIC_LIBRARY_SUFFIX}
IMPORTED_LOCATION_DEBUG ${binary_dir}/lib/Debug/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main${DEBUG_DECORATION}${CMAKE_STATIC_LIBRARY_SUFFIX}
IMPORTED_LOCATION_RELEASE ${binary_dir}/lib/Release/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main${CMAKE_STATIC_LIBRARY_SUFFIX}
IMPORTED_LOCATION_RELWITHDEBINFO ${binary_dir}/lib/RelWithDebInfo/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main${CMAKE_STATIC_LIBRARY_SUFFIX}
IMPORTED_LOCATION_MINSIZEREL ${binary_dir}/lib/MinSizeRel/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main${CMAKE_STATIC_LIBRARY_SUFFIX}
INTERFACE_LINK_LIBRARIES ${binary_dir}/lib/$<$<CONFIG:Debug>:Debug/>$<$<CONFIG:Release>:Release/>$<$<CONFIG:RelWithDebInfo>:RelWithDebInfo/>$<$<CONFIG:MinSizeRel>:MinSizeRel/>${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main${CMAKE_STATIC_LIBRARY_SUFFIX}
INTERFACE_LINK_LIBRARIES ${binary_dir}/lib/$<$<CONFIG:Debug>:Debug/>$<$<CONFIG:Release>:Release/>$<$<CONFIG:RelWithDebInfo>:RelWithDebInfo/>$<$<CONFIG:MinSizeRel>:MinSizeRel/>${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main$<$<CONFIG:Debug>:${DEBUG_DECORATION}>${CMAKE_STATIC_LIBRARY_SUFFIX}
)
endif()
endif()