diff --git a/.clang-tidy b/.clang-tidy index bfb1b252c..3515d6a2a 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -32,7 +32,6 @@ Checks: '-*, performance-inefficient-algorithm, performance-move-const-arg, performance-type-promotion-in-math-fn, - performance-unnecessary-copy-initialization, performance-unnecessary-value-param, readability-container-size-empty, readability-identifier-naming, diff --git a/CMakeLists.txt b/CMakeLists.txt index 66ae4894a..e3a44189f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -295,6 +295,12 @@ if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") set(OS_IS_MACOSX TRUE) execute_process(COMMAND uname -v OUTPUT_VARIABLE DARWIN_VERSION) string(REGEX MATCH "[0-9]+" DARWIN_VERSION ${DARWIN_VERSION}) + if(${DARWIN_VERSION} MATCHES "19") + set(MACOS_CATALINA TRUE) + set(CMAKE_XCODE_ATTRIBUTE_CLANG_CXX_LANGUAGE_STANDARD "c++14") + set(CMAKE_XCODE_ATTRIBUTE_CLANG_CXX_LIBRARY "libc++") + message(STATUS "Configuring GNSS-SDR v${VERSION} to be built on macOS Catalina 10.15") + endif() if(${DARWIN_VERSION} MATCHES "18") set(MACOS_MOJAVE TRUE) set(CMAKE_XCODE_ATTRIBUTE_CLANG_CXX_LANGUAGE_STANDARD "c++14") @@ -468,7 +474,7 @@ endif() ################################################################################ -# Set C and C++ standard +# Set minimal C and C++ standards ################################################################################ if(NOT (CMAKE_VERSION VERSION_LESS "3.1")) set(CMAKE_C_STANDARD 11) @@ -484,7 +490,7 @@ else() endif() endif() if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") - if(OS_IS_MACOSX) + if(CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang") if(CLANG_VERSION VERSION_LESS "600") add_compile_options("$<$,CXX>:-std=c++11>") else() @@ -623,132 +629,6 @@ set_package_properties(GNURADIO PROPERTIES PURPOSE "Implements flowgraph scheduler, provides some processing blocks and classes to create new ones." TYPE REQUIRED ) -if(GNURADIO_VERSION) - if(GNURADIO_VERSION VERSION_LESS ${GNSSSDR_GNURADIO_MIN_VERSION}) - unset(GNURADIO_RUNTIME_FOUND) - message(STATUS "The GNU Radio version installed in your system (v${GNURADIO_VERSION}) is too old.") - endif() -endif() -if(NOT GNURADIO_RUNTIME_FOUND) - message(STATUS "CMake cannot find GNU Radio >= ${GNSSSDR_GNURADIO_MIN_VERSION}") - if(OS_IS_LINUX) - message("Go to https://github.com/gnuradio/pybombs") - message("and follow the instructions to install GNU Radio in your system.") - endif() - if(OS_IS_MACOSX) - message("You can install it easily via Macports:") - message(" sudo port install gnuradio ") - message("Alternatively, you can use homebrew:") - message(" brew install gnuradio") - endif() - message(FATAL_ERROR "GNU Radio v${GNSSSDR_GNURADIO_MIN_VERSION} or later is required to build gnss-sdr.") -else() - if(NOT TARGET Gnuradio::runtime) - add_library(Gnuradio::runtime SHARED IMPORTED) - list(GET GNURADIO_RUNTIME_LIBRARIES 0 FIRST_DIR) - get_filename_component(GNURADIO_RUNTIME_DIR ${FIRST_DIR} ABSOLUTE) - set_target_properties(Gnuradio::runtime PROPERTIES - IMPORTED_LINK_INTERFACE_LANGUAGES "CXX" - IMPORTED_LOCATION "${GNURADIO_RUNTIME_DIR}" - INTERFACE_INCLUDE_DIRECTORIES "${GNURADIO_RUNTIME_INCLUDE_DIRS}" - INTERFACE_LINK_LIBRARIES "${GNURADIO_RUNTIME_LIBRARIES}" - ) - endif() -endif() - -if(NOT GNURADIO_ANALOG_FOUND) - message(FATAL_ERROR "*** The gnuradio-analog library v${GNSSSDR_GNURADIO_MIN_VERSION} or later is required to build gnss-sdr") -else() - if(NOT TARGET Gnuradio::analog) - add_library(Gnuradio::analog SHARED IMPORTED) - list(GET GNURADIO_ANALOG_LIBRARIES 0 FIRST_DIR) - get_filename_component(GNURADIO_ANALOG_DIR ${FIRST_DIR} ABSOLUTE) - set_target_properties(Gnuradio::analog PROPERTIES - IMPORTED_LINK_INTERFACE_LANGUAGES "CXX" - IMPORTED_LOCATION "${GNURADIO_ANALOG_DIR}" - INTERFACE_INCLUDE_DIRECTORIES "${GNURADIO_ANALOG_INCLUDE_DIRS}" - INTERFACE_LINK_LIBRARIES "${GNURADIO_ANALOG_LIBRARIES}" - ) - endif() -endif() - -if(NOT GNURADIO_BLOCKS_FOUND) - message(FATAL_ERROR "*** The gnuradio-blocks library v${GNSSSDR_GNURADIO_MIN_VERSION} or later is required to build gnss-sdr") -else() - if(NOT TARGET Gnuradio::blocks) - add_library(Gnuradio::blocks SHARED IMPORTED) - list(GET GNURADIO_BLOCKS_LIBRARIES 0 FIRST_DIR) - get_filename_component(GNURADIO_BLOCKS_DIR ${FIRST_DIR} ABSOLUTE) - set_target_properties(Gnuradio::blocks PROPERTIES - IMPORTED_LINK_INTERFACE_LANGUAGES "CXX" - IMPORTED_LOCATION "${GNURADIO_BLOCKS_DIR}" - INTERFACE_INCLUDE_DIRECTORIES "${GNURADIO_BLOCKS_INCLUDE_DIRS}" - INTERFACE_LINK_LIBRARIES "${GNURADIO_BLOCKS_LIBRARIES}" - ) - endif() -endif() - -if(NOT GNURADIO_FILTER_FOUND) - message(FATAL_ERROR "*** The gnuradio-filter library v${GNSSSDR_GNURADIO_MIN_VERSION} or later is required to build gnss-sdr") -else() - if(NOT TARGET Gnuradio::filter) - add_library(Gnuradio::filter SHARED IMPORTED) - list(GET GNURADIO_FILTER_LIBRARIES 0 FIRST_DIR) - get_filename_component(GNURADIO_FILTER_DIR ${FIRST_DIR} ABSOLUTE) - set_target_properties(Gnuradio::filter PROPERTIES - IMPORTED_LINK_INTERFACE_LANGUAGES "CXX" - IMPORTED_LOCATION "${GNURADIO_FILTER_DIR}" - INTERFACE_INCLUDE_DIRECTORIES "${GNURADIO_FILTER_INCLUDE_DIRS}" - INTERFACE_LINK_LIBRARIES "${GNURADIO_FILTER_LIBRARIES}" - ) - endif() -endif() - -if(NOT GNURADIO_FFT_FOUND) - message(FATAL_ERROR "*** The gnuradio-fft library v${GNSSSDR_GNURADIO_MIN_VERSION} or later is required to build gnss-sdr") -else() - if(NOT TARGET Gnuradio::fft) - add_library(Gnuradio::fft SHARED IMPORTED) - list(GET GNURADIO_FFT_LIBRARIES 0 FIRST_DIR) - get_filename_component(GNURADIO_FFT_DIR ${FIRST_DIR} ABSOLUTE) - set_target_properties(Gnuradio::fft PROPERTIES - IMPORTED_LINK_INTERFACE_LANGUAGES "CXX" - IMPORTED_LOCATION "${GNURADIO_FFT_DIR}" - INTERFACE_INCLUDE_DIRECTORIES "${GNURADIO_FFT_INCLUDE_DIRS}" - INTERFACE_LINK_LIBRARIES "${GNURADIO_FFT_LIBRARIES}" - ) - endif() -endif() - -if(NOT GNURADIO_PMT_FOUND) - message(FATAL_ERROR "*** The gnuradio-pmt library v${GNSSSDR_GNURADIO_MIN_VERSION} or later is required to build gnss-sdr") -else() - if(NOT TARGET Gnuradio::pmt) - add_library(Gnuradio::pmt SHARED IMPORTED) - list(GET GNURADIO_PMT_LIBRARIES 0 FIRST_DIR) - get_filename_component(GNURADIO_PMT_DIR ${FIRST_DIR} ABSOLUTE) - set_target_properties(Gnuradio::pmt PROPERTIES - IMPORTED_LINK_INTERFACE_LANGUAGES "CXX" - IMPORTED_LOCATION "${GNURADIO_PMT_DIR}" - INTERFACE_INCLUDE_DIRECTORIES "${GNURADIO_PMT_INCLUDE_DIRS}" - INTERFACE_LINK_LIBRARIES "${GNURADIO_PMT_LIBRARIES}" - ) - endif() -endif() - -if(ENABLE_UHD AND UHD_FOUND AND GNURADIO_UHD_FOUND) - if(NOT TARGET Gnuradio::uhd) - add_library(Gnuradio::uhd SHARED IMPORTED) - list(GET GNURADIO_UHD_LIBRARIES 0 FIRST_DIR) - get_filename_component(GNURADIO_UHD_DIR ${FIRST_DIR} ABSOLUTE) - set_target_properties(Gnuradio::uhd PROPERTIES - IMPORTED_LINK_INTERFACE_LANGUAGES "CXX" - IMPORTED_LOCATION "${GNURADIO_UHD_DIR}" - INTERFACE_INCLUDE_DIRECTORIES "${GNURADIO_UHD_INCLUDE_DIRS}" - INTERFACE_LINK_LIBRARIES "${GNURADIO_UHD_LIBRARIES}" - ) - endif() -endif() @@ -769,7 +649,7 @@ endif() ################################################################################ -# Dectect availability of std::filesystem +# Detect availability of std::filesystem and set C++ standard accordingly ################################################################################ set(FILESYSTEM_FOUND FALSE) if(NOT (GNURADIO_VERSION VERSION_LESS 3.8) AND LOG4CPP_READY_FOR_CXX17) @@ -787,7 +667,11 @@ if(NOT (GNURADIO_VERSION VERSION_LESS 3.8) AND LOG4CPP_READY_FOR_CXX17) TYPE OPTIONAL ) if(${FILESYSTEM_FOUND}) - set(CMAKE_CXX_STANDARD 17) + if(CMAKE_VERSION VERSION_LESS 3.12) + set(CMAKE_CXX_STANDARD 17) + else() + set(CMAKE_CXX_STANDARD 20) + endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() endif() @@ -1231,37 +1115,30 @@ if(NOT GLOG_FOUND OR ${LOCAL_GFLAGS}) set(GFLAGS_LIBRARIES_TO_LINK ${GFlags_LIBS}) set(GFLAGS_LIBRARY_DIR_TO_LINK ${GFlags_LIBRARY_DIRS}) endif() - - if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") - file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/glog-${GNSSSDR_GLOG_LOCAL_VERSION}/tmp/configure_with_gflags -"#!/bin/sh -export CPPFLAGS=-I${GFlags_INCLUDE_DIRS} -export LDFLAGS=-L${GFLAGS_LIBRARY_DIR_TO_LINK} -export LIBS=\"${GFLAGS_LIBRARIES_TO_LINK} -lc++\" -export CXXFLAGS=\"-stdlib=libc++\" -export CC=clang -export CXX=clang++ -cd ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/glog/glog-${GNSSSDR_GLOG_LOCAL_VERSION}/ -aclocal -automake --add-missing -autoreconf -vfi -cd ${CMAKE_CURRENT_BINARY_DIR}/glog-${GNSSSDR_GLOG_LOCAL_VERSION} -${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/glog/glog-${GNSSSDR_GLOG_LOCAL_VERSION}/configure" - ) - else() - file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/glog-${GNSSSDR_GLOG_LOCAL_VERSION}/tmp/configure_with_gflags -"#!/bin/sh -export CPPFLAGS=-I${GFlags_INCLUDE_DIRS} -export LDFLAGS=-L${GFLAGS_LIBRARY_DIR_TO_LINK} -export LIBS=${GFLAGS_LIBRARIES_TO_LINK} -cd ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/glog/glog-${GNSSSDR_GLOG_LOCAL_VERSION}/ -aclocal -automake --add-missing -autoreconf -vfi -cd ${CMAKE_CURRENT_BINARY_DIR}/glog-${GNSSSDR_GLOG_LOCAL_VERSION} -${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/glog/glog-${GNSSSDR_GLOG_LOCAL_VERSION}/configure" - ) + if(OS_IS_MACOSX) + set(GFLAGS_LIBRARIES_TO_LINK "${GFLAGS_LIBRARIES_TO_LINK} -lc++") + set(GLOG_EXPORT_CXX_LIBRARIES "export CXXFLAGS=\"-stdlib=libc++\"") endif() + if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(GLOG_EXPORT_C_COMPILER "export CC=clang") + set(GLOG_EXPORT_CXX_COMPILER "export CXX=clang++") + endif() + + file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/glog-${GNSSSDR_GLOG_LOCAL_VERSION}/tmp/configure_with_gflags +"#!/bin/sh +export CPPFLAGS=-I${GFlags_INCLUDE_DIRS} +export LDFLAGS=-L${GFLAGS_LIBRARY_DIR_TO_LINK} +export LIBS=\"${GFLAGS_LIBRARIES_TO_LINK}\" +${GLOG_EXPORT_CXX_LIBRARIES} +${GLOG_EXPORT_C_COMPILER} +${GLOG_EXPORT_CXX_COMPILER} +cd ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/glog/glog-${GNSSSDR_GLOG_LOCAL_VERSION}/ +aclocal +automake --add-missing +autoreconf -vfi +cd ${CMAKE_CURRENT_BINARY_DIR}/glog-${GNSSSDR_GLOG_LOCAL_VERSION} +${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/glog/glog-${GNSSSDR_GLOG_LOCAL_VERSION}/configure" + ) file(COPY ${CMAKE_CURRENT_BINARY_DIR}/glog-${GNSSSDR_GLOG_LOCAL_VERSION}/tmp/configure_with_gflags DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/glog-${GNSSSDR_GLOG_LOCAL_VERSION} @@ -1460,6 +1337,9 @@ set_package_properties(Armadillo PROPERTIES TYPE REQUIRED ) if(ARMADILLO_FOUND) + set_package_properties(Armadillo PROPERTIES + DESCRIPTION "C++ library for linear algebra and scientific computing (found: v${ARMADILLO_VERSION_STRING})" + ) if(${ARMADILLO_VERSION_STRING} VERSION_LESS ${GNSSSDR_ARMADILLO_MIN_VERSION}) set(ARMADILLO_FOUND false) set(ENABLE_OWN_ARMADILLO true) diff --git a/cmake/Modules/FindGFORTRAN.cmake b/cmake/Modules/FindGFORTRAN.cmake index f19ccd77a..395639c9a 100644 --- a/cmake/Modules/FindGFORTRAN.cmake +++ b/cmake/Modules/FindGFORTRAN.cmake @@ -45,12 +45,37 @@ find_library(GFORTRAN NAMES gfortran /usr/lib/gcc/i686-redhat-linux/4.8.2 /usr/lib/gcc/x86_64-redhat-linux/7 /usr/lib/gcc/i686-redhat-linux/7 + /usr/lib/gcc/x86_64-redhat-linux/8 + /usr/lib/gcc/i686-redhat-linux/8 + /usr/lib/gcc/x86_64-redhat-linux/9 + /usr/lib/gcc/i686-redhat-linux/9 + /usr/lib64/gcc/x86_64-redhat-linux/7 + /usr/lib64/gcc/x86_64-redhat-linux/8 + /usr/lib64/gcc/x86_64-redhat-linux/9 /usr/lib/gcc/armv7hl-redhat-linux-gnueabi/7 /usr/lib/gcc/aarch64-redhat-linux/7 /usr/lib/gcc/i586-suse-linux/4.8 # OpenSUSE 13.1 /usr/lib/gcc/i586-suse-linux/4.9 + /usr/lib/gcc/i586-suse-linux/7 + /usr/lib/gcc/i586-suse-linux/8 + /usr/lib/gcc/i586-suse-linux/9 /usr/lib/gcc/x86_64-suse-linux/4.8 /usr/lib/gcc/x86_64-suse-linux/4.9 + /usr/lib64/gcc/x86_64-suse-linux/7 + /usr/lib64/gcc/x86_64-suse-linux/8 + /usr/lib64/gcc/x86_64-suse-linux/9 + /usr/lib/gcc/armv7hl-suse-linux-gnueabi/7 + /usr/lib/gcc/armv7hl-suse-linux-gnueabi/8 + /usr/lib/gcc/armv7hl-suse-linux-gnueabi/9 + /usr/lib64/gcc/aarch64-suse-linux/7 + /usr/lib64/gcc/aarch64-suse-linux/8 + /usr/lib64/gcc/aarch64-suse-linux/9 + /usr/lib64/gcc/powerpc64-suse-linux/7 + /usr/lib64/gcc/powerpc64-suse-linux/8 + /usr/lib64/gcc/powerpc64-suse-linux/9 + /usr/lib64/gcc/powerpc64le-suse-linux/7 + /usr/lib64/gcc/powerpc64le-suse-linux/8 + /usr/lib64/gcc/powerpc64le-suse-linux/9 /usr/lib/gcc/i486-linux-gnu # Debian 7 /usr/lib/gcc/i486-linux-gnu/4.4 /usr/lib/gcc/i486-linux-gnu/4.6 @@ -142,6 +167,24 @@ find_library(GFORTRAN NAMES gfortran /usr/lib/gcc/powerpc64le-linux-gnu/8 /usr/lib/gcc/s390x-linux-gnu/8 /usr/lib/gcc/alpha-linux-gnu/8 + /usr/lib/gcc/x86_64-linux-gnu/9 + /usr/lib/gcc/aarch64-linux-gnu/9 + /usr/lib/gcc/arm-linux-gnueabi/9 + /usr/lib/gcc/arm-linux-gnueabihf/9 + /usr/lib/gcc/i686-linux-gnu/9 + /usr/lib/gcc/powerpc64le-linux-gnu/9 + /usr/lib/gcc/powerpc64-linux-gnu/9/ + /usr/lib/gcc/s390x-linux-gnu/9 + /usr/lib/gcc/alpha-linux-gnu/9 + /usr/lib/gcc/hppa-linux-gnu/9 + /usr/lib/gcc/m68k-linux-gnu/9 + /usr/lib/gcc/mips-linux-gnu/9 + /usr/lib/gcc/mips64el-linux-gnuabi64/9 + /usr/lib/gcc/mipsel-linux-gnu/9 + /usr/lib/gcc/riscv64-linux-gnu/9 + /usr/lib/gcc/sh4-linux-gnu/9 + /usr/lib/gcc/sparc64-linux-gnu/9 + /usr/lib/gcc/x86_64-linux-gnux32/9 ${GFORTRAN_ROOT}/lib $ENV{GFORTRAN_ROOT}/lib ) diff --git a/cmake/Modules/FindGNURADIO.cmake b/cmake/Modules/FindGNURADIO.cmake index 37b106833..27777983c 100644 --- a/cmake/Modules/FindGNURADIO.cmake +++ b/cmake/Modules/FindGNURADIO.cmake @@ -143,6 +143,21 @@ function(GR_MODULE EXTVAR PCNAME INCFILE LIBFILE) set(GNURADIO_FOUND FALSE) # Trick for feature_summary endif() + # Create imported target + string(TOLOWER ${EXTVAR} gnuradio_component) + if(NOT TARGET Gnuradio::${gnuradio_component}) + add_library(Gnuradio::${gnuradio_component} SHARED IMPORTED) + set(GNURADIO_LIBRARY ${GNURADIO_${EXTVAR}_LIBRARIES}) + list(GET GNURADIO_LIBRARY 0 FIRST_DIR) + get_filename_component(GNURADIO_DIR ${FIRST_DIR} ABSOLUTE) + set_target_properties(Gnuradio::${gnuradio_component} PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES "CXX" + IMPORTED_LOCATION "${GNURADIO_DIR}" + INTERFACE_INCLUDE_DIRECTORIES "${GNURADIO_${EXTVAR}_INCLUDE_DIRS}" + INTERFACE_LINK_LIBRARIES "${GNURADIO_LIBRARY}" + ) + endif() + mark_as_advanced(GNURADIO_${EXTVAR}_LIBRARIES GNURADIO_${EXTVAR}_INCLUDE_DIRS) endfunction() @@ -218,3 +233,25 @@ if(NOT DEFINED GNURADIO_FOUND) set(GNURADIO_FOUND TRUE) endif() set(GNURADIO_VERSION ${PC_GNURADIO_RUNTIME_VERSION}) + +if(NOT GNSSSDR_GNURADIO_MIN_VERSION) + set(GNSSSDR_GNURADIO_MIN_VERSION "3.7.3") +endif() + +if(GNURADIO_VERSION) + if(GNURADIO_VERSION VERSION_LESS ${GNSSSDR_GNURADIO_MIN_VERSION}) + unset(GNURADIO_RUNTIME_FOUND) + message(STATUS "The GNU Radio version installed in your system (v${GNURADIO_VERSION}) is too old.") + if(OS_IS_LINUX) + message("Go to https://github.com/gnuradio/pybombs") + message("and follow the instructions to install GNU Radio in your system.") + endif() + if(OS_IS_MACOSX) + message("You can install it easily via Macports:") + message(" sudo port install gnuradio ") + message("Alternatively, you can use homebrew:") + message(" brew install gnuradio") + endif() + message(FATAL_ERROR "GNU Radio v${GNSSSDR_GNURADIO_MIN_VERSION} or later is required to build gnss-sdr.") + endif() +endif() diff --git a/docs/changelog b/docs/changelog index 5cfa4faee..25156e270 100644 --- a/docs/changelog +++ b/docs/changelog @@ -16,7 +16,7 @@ - Improved preamble detection implementation in the decoding of navigation messages (acceleration by x1.6 on average per channel). - Shortened Acquisition to Tracking transition time. -- Applied clang-tidy checks and fixes related to performance: performance-faster-string-find, performance-inefficient-algorithm, performance-move-const-arg, performance-type-promotion-in-math-fn, performance-unnecessary-copy-initialization, performance-unnecessary-value-param, readability-string-compare. +- Applied clang-tidy checks and fixes related to performance: performance-faster-string-find, performance-inefficient-algorithm, performance-move-const-arg, performance-type-promotion-in-math-fn, performance-unnecessary-value-param, readability-string-compare. ### Improvements in Flexibility: @@ -50,7 +50,8 @@ - Added interfaces for FPGA off-loading. - CMake scripts now follow a modern approach (targets and properties) but still work with 2.8.12. - Improvements for macOS users using Homebrew. -- The volk_gnsssdr library can now be built without requiring Boost if the compiler supports C++17. +- The volk_gnsssdr library can now be built without requiring Boost if the compiler supports C++17 or higher. +- CMake scripts automatically select among C++11, C++14, C++17 or C++20 standards, the most recent as possible, depending on compiler and dependencies versions. ### Improvements in Reliability diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt index 8d515f584..4dc040d43 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt @@ -95,7 +95,11 @@ if((CMAKE_CXX_COMPILER_ID STREQUAL "GNU") AND NOT WIN32) set(CMAKE_CXX_STANDARD 14) else() if(${FILESYSTEM_FOUND}) - set(CMAKE_CXX_STANDARD 17) + if(CMAKE_VERSION VERSION_LESS 3.12) + set(CMAKE_CXX_STANDARD 17) + else() + set(CMAKE_CXX_STANDARD 20) + endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) else() set(CMAKE_CXX_STANDARD 14) @@ -134,7 +138,11 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") set(CMAKE_CXX_STANDARD 14) else() if(${FILESYSTEM_FOUND}) - set(CMAKE_CXX_STANDARD 17) + if(CMAKE_VERSION VERSION_LESS 3.12) + set(CMAKE_CXX_STANDARD 17) + else() + set(CMAKE_CXX_STANDARD 20) + endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) else() set(CMAKE_CXX_STANDARD 14) diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index fb816d0d8..e8bfb8d26 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -33,7 +33,6 @@ set(TRACKING_LIB_SOURCES cpu_multicorrelator.cc cpu_multicorrelator_real_codes.cc cpu_multicorrelator_16sc.cc - cubature_filter.cc lock_detectors.cc tcp_communication.cc tcp_packet_data.cc @@ -51,7 +50,6 @@ set(TRACKING_LIB_HEADERS cpu_multicorrelator.h cpu_multicorrelator_real_codes.h cpu_multicorrelator_16sc.h - cubature_filter.h lock_detectors.h tcp_communication.h tcp_packet_data.h @@ -65,6 +63,12 @@ set(TRACKING_LIB_HEADERS exponential_smoother.h ) +if(ARMADILLO_VERSION_STRING VERSION_GREATER 7.400) + # sqrtmat_sympd() requires 7.400 + set(TRACKING_LIB_SOURCES ${TRACKING_LIB_SOURCES} nonlinear_tracking.cc) + set(TRACKING_LIB_HEADERS ${TRACKING_LIB_HEADERS} nonlinear_tracking.h) +endif() + if(ENABLE_FPGA) set(TRACKING_LIB_SOURCES ${TRACKING_LIB_SOURCES} fpga_multicorrelator.cc dll_pll_conf_fpga.cc) set(TRACKING_LIB_HEADERS ${TRACKING_LIB_HEADERS} fpga_multicorrelator.h dll_pll_conf_fpga.h) @@ -84,6 +88,7 @@ target_link_libraries(tracking_libs Gnuradio::runtime Volkgnsssdr::volkgnsssdr core_system_parameters + algorithms_libs ${OPT_TRACKING_LIBRARIES} PRIVATE Gflags::gflags diff --git a/src/algorithms/tracking/libs/cubature_filter.cc b/src/algorithms/tracking/libs/cubature_filter.cc deleted file mode 100644 index cd7e3bb83..000000000 --- a/src/algorithms/tracking/libs/cubature_filter.cc +++ /dev/null @@ -1,199 +0,0 @@ -/*! - * \file cubature_filter.cc - * \brief Interface of a library with Bayesian noise statistic estimation - * - * Cubature_Filter implements the functionality of the Cubature Kalman - * Filter, which uses multidimensional cubature rules to estimate the - * time evolution of a nonlinear system. - * - * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE - * Transactions on Automatic Control, 54(6):1254–1269,2009. - * - * \authors - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2019 (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 . - * - * ------------------------------------------------------------------------- - */ - -#include "cubature_filter.h" - - -Cubature_filter::Cubature_filter() -{ - int nx = 1; - x_pred_out = arma::zeros(nx, 1); - P_x_pred_out = arma::eye(nx, nx) * (nx + 1); - - x_est = x_pred_out; - P_x_est = P_x_pred_out; -} - - -Cubature_filter::Cubature_filter(int nx) -{ - x_pred_out = arma::zeros(nx, 1); - P_x_pred_out = arma::eye(nx, nx) * (nx + 1); - - x_est = x_pred_out; - P_x_est = P_x_pred_out; -} - - -Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0) -{ - x_pred_out = x_pred_0; - P_x_pred_out = P_x_pred_0; - - x_est = x_pred_out; - P_x_est = P_x_pred_out; -} - - -Cubature_filter::~Cubature_filter() = default; - - -void Cubature_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0) -{ - x_pred_out = x_pred_0; - P_x_pred_out = P_x_pred_0; - - x_est = x_pred_out; - P_x_est = P_x_pred_out; -} - - -/* - * Perform the prediction step of the cubature Kalman filter - */ -void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, Model_Function* transition_fcn, const arma::mat& noise_covariance) -{ - // Compute number of cubature points - int nx = x_post.n_elem; - int np = 2 * nx; - - // Generator Matrix - arma::mat gen_one = arma::join_horiz(arma::eye(nx, nx), -1.0 * arma::eye(nx, nx)); - - // Initialize predicted mean and covariance - arma::vec x_pred = arma::zeros(nx, 1); - arma::mat P_x_pred = arma::zeros(nx, nx); - - // Factorize posterior covariance - arma::mat Sm_post = arma::chol(P_x_post, "lower"); - - // Propagate and evaluate cubature points - arma::vec Xi_post; - arma::vec Xi_pred; - - for (uint8_t i = 0; i < np; i++) - { - Xi_post = Sm_post * (std::sqrt(static_cast(np) / 2.0) * gen_one.col(i)) + x_post; - Xi_pred = (*transition_fcn)(Xi_post); - - x_pred = x_pred + Xi_pred; - P_x_pred = P_x_pred + Xi_pred * Xi_pred.t(); - } - - // Estimate predicted state and error covariance - x_pred = x_pred / static_cast(np); - P_x_pred = P_x_pred / static_cast(np) - x_pred * x_pred.t() + noise_covariance; - - // Store predicted state and error covariance - x_pred_out = x_pred; - P_x_pred_out = P_x_pred; -} - - -/* - * Perform the update step of the cubature Kalman filter - */ -void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, Model_Function* measurement_fcn, const arma::mat& noise_covariance) -{ - // Compute number of cubature points - int nx = x_pred.n_elem; - int nz = z_upd.n_elem; - int np = 2 * nx; - - // Generator Matrix - arma::mat gen_one = arma::join_horiz(arma::eye(nx, nx), -1.0 * arma::eye(nx, nx)); - - // Evaluate predicted measurement and covariances - arma::mat z_pred = arma::zeros(nz, 1); - arma::mat P_zz_pred = arma::zeros(nz, nz); - arma::mat P_xz_pred = arma::zeros(nx, nz); - - // Factorize predicted covariance - arma::mat Sm_pred = arma::chol(P_x_pred, "lower"); - - // Propagate and evaluate cubature points - arma::vec Xi_pred; - arma::vec Zi_pred; - for (uint8_t i = 0; i < np; i++) - { - Xi_pred = Sm_pred * (std::sqrt(static_cast(np) / 2.0) * gen_one.col(i)) + x_pred; - Zi_pred = (*measurement_fcn)(Xi_pred); - - z_pred = z_pred + Zi_pred; - P_zz_pred = P_zz_pred + Zi_pred * Zi_pred.t(); - P_xz_pred = P_xz_pred + Xi_pred * Zi_pred.t(); - } - - // Estimate measurement covariance and cross covariances - z_pred = z_pred / static_cast(np); - P_zz_pred = P_zz_pred / static_cast(np) - z_pred * z_pred.t() + noise_covariance; - P_xz_pred = P_xz_pred / static_cast(np) - x_pred * z_pred.t(); - - // Estimate cubature Kalman gain - arma::mat W_k = P_xz_pred * arma::inv(P_zz_pred); - - // Estimate and store the updated state and error covariance - x_est = x_pred + W_k * (z_upd - z_pred); - P_x_est = P_x_pred - W_k * P_zz_pred * W_k.t(); -} - - -arma::mat Cubature_filter::get_x_pred() const -{ - return x_pred_out; -} - - -arma::mat Cubature_filter::get_P_x_pred() const -{ - return P_x_pred_out; -} - - -arma::mat Cubature_filter::get_x_est() const -{ - return x_est; -} - - -arma::mat Cubature_filter::get_P_x_est() const -{ - return P_x_est; -} diff --git a/src/algorithms/tracking/libs/nonlinear_tracking.cc b/src/algorithms/tracking/libs/nonlinear_tracking.cc new file mode 100644 index 000000000..99d637f06 --- /dev/null +++ b/src/algorithms/tracking/libs/nonlinear_tracking.cc @@ -0,0 +1,390 @@ +/*! + * \file cubature_filter.cc + * \brief Interface of a library for nonlinear tracking algorithms + * + * Cubature_Filter implements the functionality of the Cubature Kalman + * Filter, which uses multidimensional cubature rules to estimate the + * time evolution of a nonlinear system. Unscented_filter implements + * an Unscented Kalman Filter which uses Unscented Transform rules to + * perform a similar estimation. + * + * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE + * Transactions on Automatic Control, 54(6):1254–1269,2009. + * + * \authors
    + *
  • Gerald LaMountain, 2019. gerald(at)ece.neu.edu + *
  • Jordi Vila-Valls 2019. jvila(at)cttc.es + *
+ * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2019 (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 . + * + * ------------------------------------------------------------------------- + */ + +#include "nonlinear_tracking.h" + +/***************** CUBATURE KALMAN FILTER *****************/ + +Cubature_filter::Cubature_filter() +{ + int nx = 1; + x_pred_out = arma::zeros(nx, 1); + P_x_pred_out = arma::eye(nx, nx) * (nx + 1); + + x_est = x_pred_out; + P_x_est = P_x_pred_out; +} + + +Cubature_filter::Cubature_filter(int nx) +{ + x_pred_out = arma::zeros(nx, 1); + P_x_pred_out = arma::eye(nx, nx) * (nx + 1); + + x_est = x_pred_out; + P_x_est = P_x_pred_out; +} + + +Cubature_filter::Cubature_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0) +{ + x_pred_out = x_pred_0; + P_x_pred_out = P_x_pred_0; + + x_est = x_pred_out; + P_x_est = P_x_pred_out; +} + + +Cubature_filter::~Cubature_filter() = default; + + +void Cubature_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0) +{ + x_pred_out = x_pred_0; + P_x_pred_out = P_x_pred_0; + + x_est = x_pred_out; + P_x_est = P_x_pred_out; +} + + +/* + * Perform the prediction step of the cubature Kalman filter + */ +void Cubature_filter::predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, Model_Function* transition_fcn, const arma::mat& noise_covariance) +{ + // Compute number of cubature points + int nx = x_post.n_elem; + int np = 2 * nx; + + // Generator Matrix + arma::mat gen_one = arma::join_horiz(arma::eye(nx, nx), -1.0 * arma::eye(nx, nx)); + + // Initialize predicted mean and covariance + arma::vec x_pred = arma::zeros(nx, 1); + arma::mat P_x_pred = arma::zeros(nx, nx); + + // Factorize posterior covariance + arma::mat Sm_post = arma::chol(P_x_post, "lower"); + + // Propagate and evaluate cubature points + arma::vec Xi_post; + arma::vec Xi_pred; + + for (uint8_t i = 0; i < np; i++) + { + Xi_post = Sm_post * (std::sqrt(static_cast(np) / 2.0) * gen_one.col(i)) + x_post; + Xi_pred = (*transition_fcn)(Xi_post); + + x_pred = x_pred + Xi_pred; + P_x_pred = P_x_pred + Xi_pred * Xi_pred.t(); + } + + // Compute predicted mean and error covariance + x_pred = x_pred / static_cast(np); + P_x_pred = P_x_pred / static_cast(np) - x_pred * x_pred.t() + noise_covariance; + + // Store predicted mean and error covariance + x_pred_out = x_pred; + P_x_pred_out = P_x_pred; +} + + +/* + * Perform the update step of the cubature Kalman filter + */ +void Cubature_filter::update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, Model_Function* measurement_fcn, const arma::mat& noise_covariance) +{ + // Compute number of cubature points + int nx = x_pred.n_elem; + int nz = z_upd.n_elem; + int np = 2 * nx; + + // Generator Matrix + arma::mat gen_one = arma::join_horiz(arma::eye(nx, nx), -1.0 * arma::eye(nx, nx)); + + // Initialize estimated predicted measurement and covariances + arma::mat z_pred = arma::zeros(nz, 1); + arma::mat P_zz_pred = arma::zeros(nz, nz); + arma::mat P_xz_pred = arma::zeros(nx, nz); + + // Factorize predicted covariance + arma::mat Sm_pred = arma::chol(P_x_pred, "lower"); + + // Propagate and evaluate cubature points + arma::vec Xi_pred; + arma::vec Zi_pred; + for (uint8_t i = 0; i < np; i++) + { + Xi_pred = Sm_pred * (std::sqrt(static_cast(np) / 2.0) * gen_one.col(i)) + x_pred; + Zi_pred = (*measurement_fcn)(Xi_pred); + + z_pred = z_pred + Zi_pred; + P_zz_pred = P_zz_pred + Zi_pred * Zi_pred.t(); + P_xz_pred = P_xz_pred + Xi_pred * Zi_pred.t(); + } + + // Compute measurement mean, covariance and cross covariance + z_pred = z_pred / static_cast(np); + P_zz_pred = P_zz_pred / static_cast(np) - z_pred * z_pred.t() + noise_covariance; + P_xz_pred = P_xz_pred / static_cast(np) - x_pred * z_pred.t(); + + // Compute cubature Kalman gain + arma::mat W_k = P_xz_pred * arma::inv(P_zz_pred); + + // Compute and store the updated mean and error covariance + x_est = x_pred + W_k * (z_upd - z_pred); + P_x_est = P_x_pred - W_k * P_zz_pred * W_k.t(); +} + + +arma::mat Cubature_filter::get_x_pred() const +{ + return x_pred_out; +} + + +arma::mat Cubature_filter::get_P_x_pred() const +{ + return P_x_pred_out; +} + + +arma::mat Cubature_filter::get_x_est() const +{ + return x_est; +} + + +arma::mat Cubature_filter::get_P_x_est() const +{ + return P_x_est; +} +/***************** END CUBATURE KALMAN FILTER *****************/ + + +/***************** UNSCENTED KALMAN FILTER *****************/ + +Unscented_filter::Unscented_filter() +{ + int nx = 1; + x_pred_out = arma::zeros(nx, 1); + P_x_pred_out = arma::eye(nx, nx) * (nx + 1); + + x_est = x_pred_out; + P_x_est = P_x_pred_out; +} + + +Unscented_filter::Unscented_filter(int nx) +{ + x_pred_out = arma::zeros(nx, 1); + P_x_pred_out = arma::eye(nx, nx) * (nx + 1); + + x_est = x_pred_out; + P_x_est = P_x_pred_out; +} + + +Unscented_filter::Unscented_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0) +{ + x_pred_out = x_pred_0; + P_x_pred_out = P_x_pred_0; + + x_est = x_pred_out; + P_x_est = P_x_pred_out; +} + + +Unscented_filter::~Unscented_filter() = default; + + +void Unscented_filter::initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0) +{ + x_pred_out = x_pred_0; + P_x_pred_out = P_x_pred_0; + + x_est = x_pred_out; + P_x_est = P_x_pred_out; +} + + +/* + * Perform the prediction step of the Unscented Kalman filter + */ +void Unscented_filter::predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, Model_Function* transition_fcn, const arma::mat& noise_covariance) +{ + // Compute number of sigma points + int nx = x_post.n_elem; + int np = 2 * nx + 1; + + float alpha = 0.001; + float kappa = 0.0; + float beta = 2.0; + + float lambda = std::pow(alpha, 2.0) * (static_cast(nx) + kappa) - static_cast(nx); + + // Compute UT Weights + float W0_m = lambda / (static_cast(nx) + lambda); + float W0_c = lambda / (static_cast(nx) + lambda) + (1 - std::pow(alpha, 2.0) + beta); + float Wi_m = 1.0 / (2.0 * (static_cast(nx) + lambda)); + + // Propagate and evaluate sigma points + arma::mat Xi_fact = arma::zeros(nx, nx); + arma::mat Xi_post = arma::zeros(nx, np); + arma::mat Xi_pred = arma::zeros(nx, np); + + + Xi_post.col(0) = x_post; + Xi_pred.col(0) = (*transition_fcn)(Xi_post.col(0)); + for (uint8_t i = 1; i <= nx; i++) + { + Xi_fact = std::sqrt(static_cast(nx) + lambda) * arma::sqrtmat_sympd(P_x_post); + Xi_post.col(i) = x_post + Xi_fact.col(i - 1); + Xi_post.col(i + nx) = x_post - Xi_fact.col(i - 1); + + Xi_pred.col(i) = (*transition_fcn)(Xi_post.col(i)); + Xi_pred.col(i + nx) = (*transition_fcn)(Xi_post.col(i + nx)); + } + + // Compute predicted mean + arma::vec x_pred = W0_m * Xi_pred.col(0) + Wi_m * arma::sum(Xi_pred.cols(1, np - 1), 1); + + // Compute predicted error covariance + arma::mat P_x_pred = W0_c * ((Xi_pred.col(0) - x_pred) * (Xi_pred.col(0).t() - x_pred.t())); + for (uint8_t i = 1; i < np; i++) + { + P_x_pred = P_x_pred + Wi_m * ((Xi_pred.col(i) - x_pred) * (Xi_pred.col(i).t() - x_pred.t())); + } + P_x_pred = P_x_pred + noise_covariance; + + // Store predicted mean and error covariance + x_pred_out = x_pred; + P_x_pred_out = P_x_pred; +} + + +/* + * Perform the update step of the Unscented Kalman filter + */ +void Unscented_filter::update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, Model_Function* measurement_fcn, const arma::mat& noise_covariance) +{ + // Compute number of sigma points + int nx = x_pred.n_elem; + int nz = z_upd.n_elem; + int np = 2 * nx + 1; + + float alpha = 0.001; + float kappa = 0.0; + float beta = 2.0; + + float lambda = std::pow(alpha, 2.0) * (static_cast(nx) + kappa) - static_cast(nx); + + // Compute UT Weights + float W0_m = lambda / (static_cast(nx) + lambda); + float W0_c = lambda / (static_cast(nx) + lambda) + (1.0 - std::pow(alpha, 2.0) + beta); + float Wi_m = 1.0 / (2.0 * (static_cast(nx) + lambda)); + + // Propagate and evaluate sigma points + arma::mat Xi_fact = arma::zeros(nx, nx); + arma::mat Xi_pred = arma::zeros(nx, np); + arma::mat Zi_pred = arma::zeros(nz, np); + + Xi_pred.col(0) = x_pred; + Zi_pred.col(0) = (*measurement_fcn)(Xi_pred.col(0)); + for (uint8_t i = 1; i <= nx; i++) + { + Xi_fact = std::sqrt(static_cast(nx) + lambda) * arma::sqrtmat_sympd(P_x_pred); + Xi_pred.col(i) = x_pred + Xi_fact.col(i - 1); + Xi_pred.col(i + nx) = x_pred - Xi_fact.col(i - 1); + + Zi_pred.col(i) = (*measurement_fcn)(Xi_pred.col(i)); + Zi_pred.col(i + nx) = (*measurement_fcn)(Xi_pred.col(i + nx)); + } + + // Compute measurement mean + arma::mat z_pred = W0_m * Zi_pred.col(0) + Wi_m * arma::sum(Zi_pred.cols(1, np - 1), 1); + + // Compute measurement covariance and cross covariance + arma::mat P_zz_pred = W0_c * ((Zi_pred.col(0) - z_pred) * (Zi_pred.col(0).t() - z_pred.t())); + arma::mat P_xz_pred = W0_c * ((Xi_pred.col(0) - x_pred) * (Zi_pred.col(0).t() - z_pred.t())); + for (uint8_t i = 0; i < np; i++) + { + P_zz_pred = P_zz_pred + Wi_m * ((Zi_pred.col(i) - z_pred) * (Zi_pred.col(i).t() - z_pred.t())); + P_xz_pred = P_xz_pred + Wi_m * ((Xi_pred.col(i) - x_pred) * (Zi_pred.col(i).t() - z_pred.t())); + } + P_zz_pred = P_zz_pred + noise_covariance; + + // Estimate cubature Kalman gain + arma::mat W_k = P_xz_pred * arma::inv(P_zz_pred); + + // Estimate and store the updated mean and error covariance + x_est = x_pred + W_k * (z_upd - z_pred); + P_x_est = P_x_pred - W_k * P_zz_pred * W_k.t(); +} + + +arma::mat Unscented_filter::get_x_pred() const +{ + return x_pred_out; +} + + +arma::mat Unscented_filter::get_P_x_pred() const +{ + return P_x_pred_out; +} + + +arma::mat Unscented_filter::get_x_est() const +{ + return x_est; +} + + +arma::mat Unscented_filter::get_P_x_est() const +{ + return P_x_est; +} + +/***************** END UNSCENTED KALMAN FILTER *****************/ diff --git a/src/algorithms/tracking/libs/cubature_filter.h b/src/algorithms/tracking/libs/nonlinear_tracking.h similarity index 63% rename from src/algorithms/tracking/libs/cubature_filter.h rename to src/algorithms/tracking/libs/nonlinear_tracking.h index 87f45bcdd..fdba4fec3 100644 --- a/src/algorithms/tracking/libs/cubature_filter.h +++ b/src/algorithms/tracking/libs/nonlinear_tracking.h @@ -1,10 +1,12 @@ /*! - * \file cubature_filter.h - * \brief Interface of a library with Bayesian noise statistic estimation + * \file nonlinear_tracking.h + * \brief Interface of a library for nonlinear tracking algorithms * * Cubature_Filter implements the functionality of the Cubature Kalman * Filter, which uses multidimensional cubature rules to estimate the - * time evolution of a nonlinear system. + * time evolution of a nonlinear system. Unscented_filter implements + * an Unscented Kalman Filter which uses Unscented Transform rules to + * perform a similar estimation. * * [1] I Arasaratnam and S Haykin. Cubature kalman filters. IEEE * Transactions on Automatic Control, 54(6):1254–1269,2009. @@ -38,18 +40,19 @@ * ------------------------------------------------------------------------- */ -#ifndef GNSS_SDR_CUBATURE_FILTER_H_ -#define GNSS_SDR_CUBATURE_FILTER_H_ +#ifndef GNSS_SDR_NONLINEAR_TRACKING_H_ +#define GNSS_SDR_NONLINEAR_TRACKING_H_ #include #include // Abstract model function -class Model_Function{ - public: - Model_Function() {}; - virtual arma::vec operator() (arma::vec input) = 0; - virtual ~Model_Function() = default; +class Model_Function +{ +public: + Model_Function(){}; + virtual arma::vec operator()(arma::vec input) = 0; + virtual ~Model_Function() = default; }; class Cubature_filter @@ -81,4 +84,33 @@ private: arma::mat P_x_est; }; +class Unscented_filter +{ +public: + // Constructors and destructors + Unscented_filter(); + Unscented_filter(int nx); + Unscented_filter(const arma::vec& x_pred_0, const arma::mat& P_x_pred_0); + ~Unscented_filter(); + + // Reinitialization function + void initialize(const arma::mat& x_pred_0, const arma::mat& P_x_pred_0); + + // Prediction and estimation + void predict_sequential(const arma::vec& x_post, const arma::mat& P_x_post, Model_Function* transition_fcn, const arma::mat& noise_covariance); + void update_sequential(const arma::vec& z_upd, const arma::vec& x_pred, const arma::mat& P_x_pred, Model_Function* measurement_fcn, const arma::mat& noise_covariance); + + // Getters + arma::mat get_x_pred() const; + arma::mat get_P_x_pred() const; + arma::mat get_x_est() const; + arma::mat get_P_x_est() const; + +private: + arma::vec x_pred_out; + arma::mat P_x_pred_out; + arma::vec x_est; + arma::mat P_x_est; +}; + #endif diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 8eb57397e..12f65f0fd 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -161,6 +161,11 @@ if(ENABLE_FPGA) add_definitions(-DFPGA_BLOCKS_TEST=1) endif() +if(ARMADILLO_VERSION_STRING VERSION_GREATER 8.400) + # mvnrnd() requires 8.400 + add_definitions(-DARMADILLO_HAVE_MVNRND=1) +endif() + find_package(Gnuplot) if(GNUPLOT_FOUND) add_definitions(-DGNUPLOT_EXECUTABLE="${GNUPLOT_EXECUTABLE}") @@ -793,13 +798,20 @@ endif() ######################################################### if(NOT ENABLE_PACKAGING AND NOT ENABLE_FPGA) + set(NONLINEAR_SOURCES "") + if(ARMADILLO_VERSION_STRING VERSION_GREATER 8.400) + set(NONLINEAR_SOURCES + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc + ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc + ) + endif() add_executable(trk_test ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc - ${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc + ${NONLINEAR_SOURCES} ) if(${FILESYSTEM_FOUND}) target_compile_definitions(trk_test PRIVATE -DHAS_STD_FILESYSTEM=1) diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index d22149fee..e20338cfa 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -99,7 +99,10 @@ DECLARE_string(log_dir); #endif #include "unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc" +#if ARMADILLO_HAVE_MVNRND #include "unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc" +#include "unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc" +#endif #include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc" #include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_test.cc" #include "unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc" diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc index 948e5cc69..ec61ea47d 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/cubature_filter_test.cc @@ -28,12 +28,13 @@ * ------------------------------------------------------------------------- */ -#include "cubature_filter.h" +#include "nonlinear_tracking.h" #include #include #include #define CUBATURE_TEST_N_TRIALS 1000 +#define CUBATURE_TEST_TOLERANCE 0.01 class Transition_Model : public Model_Function { @@ -127,8 +128,8 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest) kf_x_pre = kf_F * kf_x_post; kf_P_x_pre = kf_F * kf_P_x_post * kf_F.t() + kf_Q; - EXPECT_TRUE(arma::approx_equal(ckf_x_pre, kf_x_pre, "absdiff", 0.01)); - EXPECT_TRUE(arma::approx_equal(ckf_P_x_pre, kf_P_x_pre, "absdiff", 0.01)); + EXPECT_TRUE(arma::approx_equal(ckf_x_pre, kf_x_pre, "absdiff", CUBATURE_TEST_TOLERANCE)); + EXPECT_TRUE(arma::approx_equal(ckf_P_x_pre, kf_P_x_pre, "absdiff", CUBATURE_TEST_TOLERANCE)); // Update Step kf_H = arma::randu(ny, nx); @@ -151,8 +152,8 @@ TEST(CubatureFilterComputationTest, CubatureFilterTest) kf_x_post = kf_x_pre + kf_K * (kf_y - kf_H * kf_x_pre); kf_P_x_post = (arma::eye(nx, nx) - kf_K * kf_H) * kf_P_x_pre; - EXPECT_TRUE(arma::approx_equal(ckf_x_post, kf_x_post, "absdiff", 0.01)); - EXPECT_TRUE(arma::approx_equal(ckf_P_x_post, kf_P_x_post, "absdiff", 0.01)); + EXPECT_TRUE(arma::approx_equal(ckf_x_post, kf_x_post, "absdiff", CUBATURE_TEST_TOLERANCE)); + EXPECT_TRUE(arma::approx_equal(ckf_P_x_post, kf_P_x_post, "absdiff", CUBATURE_TEST_TOLERANCE)); delete transition_function; delete measurement_function; diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc new file mode 100644 index 000000000..d68a76ea6 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/unscented_filter_test.cc @@ -0,0 +1,161 @@ +/*! + * \file unscented_filter_test.cc + * \brief This file implements numerical accuracy test for the CKF library. + * \author Gerald LaMountain, 2019. gerald(at)ece.neu.edu + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2019 (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 . + * + * ------------------------------------------------------------------------- + */ + +#include "nonlinear_tracking.h" +#include +#include +#include + +#define UNSCENTED_TEST_N_TRIALS 10 +#define UNSCENTED_TEST_TOLERANCE 10 + +class Transition_Model_UKF : public Model_Function +{ +public: + Transition_Model_UKF(arma::mat kf_F) { coeff_mat = kf_F; }; + virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; }; + +private: + arma::mat coeff_mat; +}; + +class Measurement_Model_UKF : public Model_Function +{ +public: + Measurement_Model_UKF(arma::mat kf_H) { coeff_mat = kf_H; }; + virtual arma::vec operator()(arma::vec input) { return coeff_mat * input; }; + +private: + arma::mat coeff_mat; +}; + +TEST(UnscentedFilterComputationTest, UnscentedFilterTest) +{ + Unscented_filter kf_unscented; + + arma::vec kf_x; + arma::mat kf_P_x; + + arma::vec kf_x_pre; + arma::mat kf_P_x_pre; + + arma::vec ukf_x_pre; + arma::mat ukf_P_x_pre; + + arma::vec kf_x_post; + arma::mat kf_P_x_post; + + arma::vec ukf_x_post; + arma::mat ukf_P_x_post; + + arma::mat kf_F; + arma::mat kf_H; + + arma::mat kf_Q; + arma::mat kf_R; + + arma::vec eta; + arma::vec nu; + + arma::vec kf_y; + arma::mat kf_P_y; + arma::mat kf_K; + + Model_Function* transition_function; + Model_Function* measurement_function; + + //--- Perform initializations ------------------------------ + + std::random_device r; + std::default_random_engine e1(r()); + std::normal_distribution normal_dist(0, 5); + std::uniform_real_distribution uniform_dist(0.1, 5.0); + + uint8_t nx = 0; + uint8_t ny = 0; + + for (uint16_t k = 0; k < UNSCENTED_TEST_N_TRIALS; k++) + { + nx = std::rand() % 5 + 1; + ny = std::rand() % 5 + 1; + + kf_x = arma::randn(nx, 1); + + kf_P_x_post = 5.0 * arma::diagmat(arma::randu(nx, 1)); + kf_x_post = arma::mvnrnd(kf_x, kf_P_x_post); + + kf_unscented.initialize(kf_x_post, kf_P_x_post); + + // Prediction Step + kf_F = arma::randu(nx, nx); + kf_Q = arma::diagmat(arma::randu(nx, 1)); + + transition_function = new Transition_Model_UKF(kf_F); + arma::mat ttx = (*transition_function)(kf_x_post); + + kf_unscented.predict_sequential(kf_x_post, kf_P_x_post, transition_function, kf_Q); + + ukf_x_pre = kf_unscented.get_x_pred(); + ukf_P_x_pre = kf_unscented.get_P_x_pred(); + + kf_x_pre = kf_F * kf_x_post; + kf_P_x_pre = kf_F * kf_P_x_post * kf_F.t() + kf_Q; + + EXPECT_TRUE(arma::approx_equal(ukf_x_pre, kf_x_pre, "absdiff", UNSCENTED_TEST_TOLERANCE)); + EXPECT_TRUE(arma::approx_equal(ukf_P_x_pre, kf_P_x_pre, "absdiff", UNSCENTED_TEST_TOLERANCE)); + + // Update Step + kf_H = arma::randu(ny, nx); + kf_R = arma::diagmat(arma::randu(ny, 1)); + + eta = arma::mvnrnd(arma::zeros(nx, 1), kf_Q); + nu = arma::mvnrnd(arma::zeros(ny, 1), kf_R); + + kf_y = kf_H * (kf_F * kf_x + eta) + nu; + + measurement_function = new Measurement_Model_UKF(kf_H); + kf_unscented.update_sequential(kf_y, kf_x_pre, kf_P_x_pre, measurement_function, kf_R); + + ukf_x_post = kf_unscented.get_x_est(); + ukf_P_x_post = kf_unscented.get_P_x_est(); + + kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; + kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); + + kf_x_post = kf_x_pre + kf_K * (kf_y - kf_H * kf_x_pre); + kf_P_x_post = (arma::eye(nx, nx) - kf_K * kf_H) * kf_P_x_pre; + + EXPECT_TRUE(arma::approx_equal(ukf_x_post, kf_x_post, "absdiff", UNSCENTED_TEST_TOLERANCE)); + EXPECT_TRUE(arma::approx_equal(ukf_P_x_post, kf_P_x_post, "absdiff", UNSCENTED_TEST_TOLERANCE)); + + delete transition_function; + delete measurement_function; + } +}