diff --git a/.autotools b/.autotools
new file mode 100644
index 0000000000000000000000000000000000000000..52426e6ee238273b9bedea21c0da52873d4b0bfb
--- /dev/null
+++ b/.autotools
@@ -0,0 +1,83 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<configurations>
+<configuration id="org.eclipse.linuxtools.cdt.autotools.core.configuration.build.1456028388">
+<option id="configure" value="configure"/>
+<option id="configdir" value=""/>
+<option id="cache-file" value=""/>
+<option id="help" value="false"/>
+<option id="no-create" value="false"/>
+<option id="quiet" value="false"/>
+<option id="version" value="false"/>
+<option id="host" value=""/>
+<option id="build" value=""/>
+<option id="target" value=""/>
+<option id="prefix" value=""/>
+<option id="exec-prefix" value=""/>
+<option id="libdir" value=""/>
+<option id="bindir" value=""/>
+<option id="sbindir" value=""/>
+<option id="includedir" value=""/>
+<option id="datadir" value=""/>
+<option id="sysconfdir" value=""/>
+<option id="infodir" value=""/>
+<option id="mandir" value=""/>
+<option id="srcdir" value=""/>
+<option id="localstatedir" value=""/>
+<option id="sharedstatedir" value=""/>
+<option id="libexecdir" value=""/>
+<option id="oldincludedir" value=""/>
+<option id="program-prefix" value=""/>
+<option id="program-suffix" value=""/>
+<option id="program-transform-name" value=""/>
+<option id="env_vars" value=""/>
+<option id="enable-maintainer-mode" value="false"/>
+<flag id="CFLAGS" value="CFLAGS|CXXFLAGS">
+<flagvalue id="cflags-debug" value="false"/>
+<flagvalue id="cflags-gprof" value="false"/>
+<flagvalue id="cflags-gcov" value="false"/>
+</flag>
+<option id="user" value=""/>
+<option id="autogen" value="autogen.sh"/>
+<option id="autogenOpts" value=""/>
+</configuration>
+<configuration id="org.eclipse.linuxtools.cdt.autotools.core.configuration.build.debug.116232725">
+<option id="configure" value="configure"/>
+<option id="configdir" value=""/>
+<option id="cache-file" value=""/>
+<option id="help" value="false"/>
+<option id="no-create" value="false"/>
+<option id="quiet" value="false"/>
+<option id="version" value="false"/>
+<option id="host" value=""/>
+<option id="build" value=""/>
+<option id="target" value=""/>
+<option id="prefix" value=""/>
+<option id="exec-prefix" value=""/>
+<option id="libdir" value=""/>
+<option id="bindir" value=""/>
+<option id="sbindir" value=""/>
+<option id="includedir" value=""/>
+<option id="datadir" value=""/>
+<option id="sysconfdir" value=""/>
+<option id="infodir" value=""/>
+<option id="mandir" value=""/>
+<option id="srcdir" value=""/>
+<option id="localstatedir" value=""/>
+<option id="sharedstatedir" value=""/>
+<option id="libexecdir" value=""/>
+<option id="oldincludedir" value=""/>
+<option id="program-prefix" value=""/>
+<option id="program-suffix" value=""/>
+<option id="program-transform-name" value=""/>
+<option id="env_vars" value=""/>
+<option id="enable-maintainer-mode" value="false"/>
+<flag id="CFLAGS" value="CFLAGS|CXXFLAGS">
+<flagvalue id="cflags-debug" value="false"/>
+<flagvalue id="cflags-gprof" value="false"/>
+<flagvalue id="cflags-gcov" value="false"/>
+</flag>
+<option id="user" value="CFLAGS='-g -O0' CXXFLAGS='-g -O0'"/>
+<option id="autogen" value="autogen.sh"/>
+<option id="autogenOpts" value=""/>
+</configuration>
+</configurations>
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
new file mode 100644
index 0000000000000000000000000000000000000000..697259cc7409fa76ecc080bad109554c15c9c16e
--- /dev/null
+++ b/.gitlab-ci.yml
@@ -0,0 +1,99 @@
+stages:
+ - license
+ - build_and_test
+
+############ YAML ANCHORS ############
+.preliminaries_template: &preliminaries_definition
+ ## Install ssh-agent if not already installed, it is required by Docker.
+ ## (change apt-get to yum if you use an RPM-based image)
+ - 'which ssh-agent || ( apt-get update -y && apt-get install openssh-client -y )'
+
+ ## Run ssh-agent (inside the build environment)
+ - eval $(ssh-agent -s)
+
+ ## Add the SSH key stored in SSH_PRIVATE_KEY variable to the agent store
+ ## We're using tr to fix line endings which makes ed25519 keys work
+ ## without extra base64 encoding.
+ ## https://gitlab.com/gitlab-examples/ssh-private-key/issues/1#note_48526556
+ - mkdir -p ~/.ssh
+ - chmod 700 ~/.ssh
+ - echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - > /dev/null
+ # - echo "$SSH_KNOWN_HOSTS" > $HOME/.ssh/known_hosts
+ - ssh-keyscan -H -p 2202 gitlab.iri.upc.edu >> $HOME/.ssh/known_hosts
+
+ # update apt
+ - apt-get update
+
+.license_header_template: &license_header_definition
+ - cd $CI_PROJECT_DIR
+
+ # configure git
+ - export CI_NEW_BRANCH=ci_processing$RANDOM
+ - echo creating new temporary branch... $CI_NEW_BRANCH
+ #- export CI_NEW_BRANCH=ci_processing$CI_COMMIT_SHORT_SHA
+ - git config --global user.email "${CI_EMAIL}"
+ - git config --global user.name "${CI_USERNAME}"
+ - git checkout -b $CI_NEW_BRANCH # temporary branch
+
+ # license headers
+ - export CURRENT_YEAR=$( date +'%Y' )
+ - echo "current year:" ${CURRENT_YEAR}
+ - cd scripts
+ - if [ -f license_header_${CURRENT_YEAR}.txt ]; then
+ # add license headers to new files
+ - echo "File license_header_${CURRENT_YEAR}.txt already exists. License headers are assumed to be updated. Adding headers to new files..."
+ - ./license_manager.sh --add --path=${CI_PROJECT_DIR} --license-header=license_header_${CURRENT_YEAR}.txt
+ - else
+ # update license headers of all files
+ - export PREV_YEAR=$(( CURRENT_YEAR-1 ))
+ - echo "Creating new file license_header_${CURRENT_YEAR}.txt..."
+ - git mv license_header_${PREV_YEAR}.txt license_header_${CURRENT_YEAR}.txt
+ - sed -i "s/${PREV_YEAR}/${PREV_YEAR},${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt
+ - ./license_manager.sh --update --path=${CI_PROJECT_DIR} --license-header=license_header_${CURRENT_YEAR}.txt
+ - fi
+ - cd ..
+
+ # push changes (if any)
+ - if git commit -a -m "[skip ci] license headers added or modified" ; then
+ - git remote set-url --push origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git"
+ - git push origin $CI_NEW_BRANCH:${CI_COMMIT_REF_NAME}
+ - else
+ - echo "No changes, nothing to commit!"
+ - fi
+
+.build_and_test_template: &build_and_test_definition
+ - cd $CI_PROJECT_DIR
+ - git submodule update --init
+ - mkdir -pv build
+ - cd build
+ - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON ..
+ - make -j2
+ - ctest -j2
+ - make install
+
+############ LICENSE HEADERS ############
+license_headers:
+ stage: license
+ image: labrobotica/wolf_deps:16.04
+ before_script:
+ - *preliminaries_definition
+ script:
+ - *license_header_definition
+
+############ UBUNTU 16.04 TESTS ############
+build_and_test_none:xenial:
+ stage: build_and_test
+ image: labrobotica/wolf_vision_deps:16.04
+ before_script:
+ - *preliminaries_definition
+ script:
+ - *build_and_test_definition
+
+############ UBUNTU 18.04 TESTS ############
+build_and_test_none:bionic:
+ stage: build_and_test
+ image: labrobotica/wolf_vision_deps:18.04
+ before_script:
+ - *preliminaries_definition
+ script:
+ - *build_and_test_definition
\ No newline at end of file
diff --git a/.gitmodules b/.gitmodules
index 2999783577610c44e39242e31c38b22559de61b4..67ac92ab606b3e7908bd301c76b63e9fa78b0b58 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,4 +1,4 @@
[submodule "deps/RTKLIB"]
path = deps/RTKLIB
- url = ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/gauss_project/RTKLIB.git
- branch = demo5
+ url = https://gitlab.iri.upc.edu/mobile_robotics/gauss_project/RTKLIB.git
+ branch = devel
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6ddfb998b6c1640d5fb46d6a57d8a469f5b74b06..5d81f04804bcc35bc58b360cc50f69f49eb91898 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,8 @@
+#Start gnss-utils build
+MESSAGE("Starting gnss-utils CMakeLists ...")
+
# Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 2.4)
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
if(COMMAND cmake_policy)
cmake_policy(SET CMP0005 NEW)
@@ -7,8 +10,11 @@ if(COMMAND cmake_policy)
endif(COMMAND cmake_policy)
+# MAC OSX RPATH
+set(CMAKE_MACOSX_RPATH true)
+
# The project name and the type of project
-PROJECT(gnss-utils)
+PROJECT(gnss_utils)
SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
@@ -35,7 +41,127 @@ else()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()
-ADD_SUBDIRECTORY(src)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fpermissive")
+
+FIND_PACKAGE(Eigen3 3.3 REQUIRED)
+IF(Eigen3_FOUND)
+ INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
+ENDIF(Eigen3_FOUND)
+
+
+# Testing
+IF(NOT BUILD_TESTS)
+ OPTION(BUILD_TESTS "Build Unit tests" ON)
+ENDIF(NOT BUILD_TESTS)
+if(BUILD_TESTS)
+ # Enables testing for this directory and below.
+ # Note that ctest expects to find a test file in the build directory root.
+ # Therefore, this command should be in the source directory root.
+ #include(CTest) # according to http://public.kitware.com/pipermail/cmake/2012-June/050853.html
+ MESSAGE("Building tests.")
+ enable_testing()
+ set(_GNSS_UTILS_ROOT_DIR ${CMAKE_SOURCE_DIR})
+endif()
+
+# Define the directory where will be the configured config.h
+SET(GNSS_UTILS_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/gnss_utils/internal)
+
+# Create the specified output directory if it does not exist.
+IF(NOT EXISTS "${GNSS_UTILS_CONFIG_DIR}")
+ message(STATUS "Creating config output directory: ${GNSS_UTILS_CONFIG_DIR}")
+ file(MAKE_DIRECTORY "${GNSS_UTILS_CONFIG_DIR}")
+ENDIF()
+IF(EXISTS "${GNSS_UTILS_CONFIG_DIR}" AND NOT IS_DIRECTORY "${GNSS_UTILS_CONFIG_DIR}")
+ message(FATAL_ERROR "Bug: Specified CONFIG_DIR: "
+ "${GNSS_UTILS_CONFIG_DIR} exists, but is not a directory.")
+ENDIF()
+# Configure config.h
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${GNSS_UTILS_CONFIG_DIR}/config.h")
+message("GNSS_UTILS CONFIG ${GNSS_UTILS_CONFIG_DIR}/config.h")
+message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
+include_directories("${PROJECT_BINARY_DIR}/conf")
+
+# rtklib path
+SET(RTKLIB_DIR deps/RTKLIB)
+SET(RTKLIB_SRC_DIR ${RTKLIB_DIR}/src)
+
+# driver source files
+SET(SOURCES
+ src/navigation.cpp
+ src/observations.cpp
+ src/range.cpp
+ src/receiver_raw_base.cpp
+ src/receivers/ublox_raw.cpp
+ src/receivers/novatel_raw.cpp
+ src/snapshot.cpp
+ src/tdcp.cpp
+ src/utils/utils.cpp
+ src/utils/transformations.cpp
+ src/utils/rcv_position.cpp
+ src/utils/satellite.cpp)
+
+SET(RTKLIB_SRC
+ ${RTKLIB_SRC_DIR}/pntpos.c
+ ${RTKLIB_SRC_DIR}/rtkcmn.c
+ ${RTKLIB_SRC_DIR}/sbas.c
+ ${RTKLIB_SRC_DIR}/ephemeris.c
+ ${RTKLIB_SRC_DIR}/preceph.c
+ ${RTKLIB_SRC_DIR}/qzslex.c
+ ${RTKLIB_SRC_DIR}/rtcm.c
+ ${RTKLIB_SRC_DIR}/rtcm2.c
+ ${RTKLIB_SRC_DIR}/rtcm3.c
+ ${RTKLIB_SRC_DIR}/rtcm3e.c
+ ${RTKLIB_SRC_DIR}/ionex.c
+ ${RTKLIB_SRC_DIR}/rinex.c
+ ${RTKLIB_SRC_DIR}/rcvraw.c
+ ${RTKLIB_SRC_DIR}/rcv/binex.c
+ ${RTKLIB_SRC_DIR}/rcv/cmr.c
+ ${RTKLIB_SRC_DIR}/rcv/comnav.c
+ ${RTKLIB_SRC_DIR}/rcv/crescent.c
+ ${RTKLIB_SRC_DIR}/rcv/gw10.c
+ ${RTKLIB_SRC_DIR}/rcv/javad.c
+ ${RTKLIB_SRC_DIR}/rcv/novatel.c
+ ${RTKLIB_SRC_DIR}/rcv/nvs.c
+ ${RTKLIB_SRC_DIR}/rcv/rt17.c
+ ${RTKLIB_SRC_DIR}/rcv/septentrio.c
+ ${RTKLIB_SRC_DIR}/rcv/skytraq.c
+ ${RTKLIB_SRC_DIR}/rcv/swiftnav.c
+ ${RTKLIB_SRC_DIR}/rcv/tersus.c
+ ${RTKLIB_SRC_DIR}/rcv/ublox.c)
+
+# application header files
+# NOT SET SINCE WE COPY THE WHOLE include FOLDER
+
+# Eigen #######
+FIND_PACKAGE(Eigen3 REQUIRED)
+
+# Adding libraries' directories
+link_directories(/usr/lib/x86_64-linux-gnu/)
+
+# Adding include directories
+INCLUDE_DIRECTORIES(include/ ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${RTKLIB_SRC_DIR})
+
+# create the shared library
+ADD_LIBRARY(gnss_utils SHARED ${SOURCES} ${RTKLIB_SRC})
+TARGET_LINK_LIBRARIES(gnss_utils ${Boost_LIBRARIES})
+
+# Examples
+ADD_SUBDIRECTORY(src/examples)
+# Testing
+if(BUILD_TESTS)
+ add_subdirectory(test)
+endif()
+
+# Installing
+INSTALL(TARGETS ${PROJECT_NAME}
+ RUNTIME DESTINATION bin
+ LIBRARY DESTINATION lib
+ ARCHIVE DESTINATION lib)
+INSTALL(DIRECTORY include/gnss_utils DESTINATION include/iri-algorithms)
+INSTALL(FILES ${RTKLIB_SRC_DIR}/rtklib.h DESTINATION include/iri-algorithms/gnss_utils)
+INSTALL(FILES Findgnss_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
+INSTALL(FILES "${GNSS_UTILS_CONFIG_DIR}/config.h"
+ DESTINATION include/iri-algorithms/gnss_utils/internal)
FIND_PACKAGE(Doxygen)
diff --git a/Findgnss_utils.cmake b/Findgnss_utils.cmake
index 85306e55c56ff0aea7537dcf31d091f2a426d9dc..9d725b5ebf40de6df5168645034348bba16c464f 100644
--- a/Findgnss_utils.cmake
+++ b/Findgnss_utils.cmake
@@ -1,17 +1,36 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(gnss_utils_INCLUDE_DIR gnss_utils.h obervation.h /usr/include/iridrivers /usr/local/lib)
+FIND_PATH(
+ gnss_utils_INCLUDE_DIRS
+ NAMES gnss_utils.h
+ PATHS /usr/local/include/iri-algorithms/gnss_utils /usr/local/lib/iri-algorithms/gnss_utils)
+#change INCLUDE_DIRS to its parent directory (to force includes with libname in front
+get_filename_component(gnss_utils_INCLUDE_DIRS ${gnss_utils_INCLUDE_DIRS} DIRECTORY)
+IF(gnss_utils_INCLUDE_DIRS)
+ MESSAGE("Found gnss_utils include dirs: ${gnss_utils_INCLUDE_DIRS}")
+ELSE(gnss_utils_INCLUDE_DIRS)
+ MESSAGE("Couldn't find gnss_utils include dirs")
+ENDIF(gnss_utils_INCLUDE_DIRS)
-FIND_LIBRARY(gnss_utils_LIBRARY
- NAMES gnss_utils
- PATHS /usr/lib /usr/local/lib /usr/local/lib)
+FIND_LIBRARY(
+ gnss_utils_LIBRARIES
+ NAMES libgnss_utils.so libgnss_utils.dylib
+ PATHS /usr/local/lib/iri-algorithms)
+IF(gnss_utils_LIBRARIES)
+ MESSAGE("Found gnss_utils lib: ${gnss_utils_LIBRARIES}")
+ELSE(gnss_utils_LIBRARIES)
+ MESSAGE("Couldn't find gnss_utils lib")
+ENDIF(gnss_utils_LIBRARIES)
-IF (gnss_utils_INCLUDE_DIR AND gnss_utils_LIBRARY)
+IF (gnss_utils_INCLUDE_DIRS AND gnss_utils_LIBRARIES)
SET(gnss_utils_FOUND TRUE)
-ENDIF (gnss_utils_INCLUDE_DIR AND gnss_utils_LIBRARY)
+ SET(gnss_utils_INCLUDE_DIR ${gnss_utils_INCLUDE_DIRS})
+ SET(gnss_utils_LIBRARY ${gnss_utils_LIBRARIES})
+ ELSE(gnss_utils_INCLUDE_DIRS AND gnss_utils_LIBRARIES)
+ set(gnss_utils_FOUND FALSE)
+ENDIF (gnss_utils_INCLUDE_DIRS AND gnss_utils_LIBRARIES)
IF (gnss_utils_FOUND)
IF (NOT gnss_utils_FIND_QUIETLY)
- MESSAGE(STATUS "Found gnss_utils: ${gnss_utils_LIBRARY}")
+ MESSAGE(STATUS "Found gnss_utils: ${gnss_utils_LIBRARIES}")
ENDIF (NOT gnss_utils_FIND_QUIETLY)
ELSE (gnss_utils_FOUND)
IF (gnss_utils_FIND_REQUIRED)
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000000000000000000000000000000000000..281d399f13dfd23087acc56303dd38d68162587c
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,619 @@
+ GNU GENERAL PUBLIC LICENSE
+ Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+ Preamble
+
+ The GNU General Public License is a free, copyleft license for
+software and other kinds of works.
+
+ The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works. By contrast,
+the GNU General Public License is intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains free
+software for all its users. We, the Free Software Foundation, use the
+GNU General Public License for most of our software; it applies also to
+any other work released this way by its authors. You can apply it to
+your programs, too.
+
+ When we speak of free software, we are referring to freedom, not
+price. Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.
+
+ To protect your rights, we need to prevent others from denying you
+these rights or asking you to surrender the rights. Therefore, you have
+certain responsibilities if you distribute copies of the software, or if
+you modify it: responsibilities to respect the freedom of others.
+
+ For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must pass on to the recipients the same
+freedoms that you received. You must make sure that they, too, receive
+or can get the source code. And you must show them these terms so they
+know their rights.
+
+ Developers that use the GNU GPL protect your rights with two steps:
+(1) assert copyright on the software, and (2) offer you this License
+giving you legal permission to copy, distribute and/or modify it.
+
+ For the developers' and authors' protection, the GPL clearly explains
+that there is no warranty for this free software. For both users' and
+authors' sake, the GPL requires that modified versions be marked as
+changed, so that their problems will not be attributed erroneously to
+authors of previous versions.
+
+ Some devices are designed to deny users access to install or run
+modified versions of the software inside them, although the manufacturer
+can do so. This is fundamentally incompatible with the aim of
+protecting users' freedom to change the software. The systematic
+pattern of such abuse occurs in the area of products for individuals to
+use, which is precisely where it is most unacceptable. Therefore, we
+have designed this version of the GPL to prohibit the practice for those
+products. If such problems arise substantially in other domains, we
+stand ready to extend this provision to those domains in future versions
+of the GPL, as needed to protect the freedom of users.
+
+ Finally, every program is threatened constantly by software patents.
+States should not allow patents to restrict development and use of
+software on general-purpose computers, but in those that do, we wish to
+avoid the special danger that patents applied to a free program could
+make it effectively proprietary. To prevent this, the GPL assures that
+patents cannot be used to render the program non-free.
+
+ The precise terms and conditions for copying, distribution and
+modification follow.
+
+ TERMS AND CONDITIONS
+
+ 0. Definitions.
+
+ "This License" refers to version 3 of the GNU General Public License.
+
+ "Copyright" also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.
+
+ "The Program" refers to any copyrightable work licensed under this
+License. Each licensee is addressed as "you". "Licensees" and
+"recipients" may be individuals or organizations.
+
+ To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy. The resulting work is called a "modified version" of the
+earlier work or a work "based on" the earlier work.
+
+ A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+ To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy. Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+ To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies. Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.
+
+ An interactive user interface displays "Appropriate Legal Notices"
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License. If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+ 1. Source Code.
+
+ The "source code" for a work means the preferred form of the work
+for making modifications to it. "Object code" means any non-source
+form of a work.
+
+ A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+ The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form. A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+ The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities. However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work. For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+ The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.
+
+ The Corresponding Source for a work in source code form is that
+same work.
+
+ 2. Basic Permissions.
+
+ All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met. This License explicitly affirms your unlimited
+permission to run the unmodified Program. The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work. This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+ You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force. You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright. Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+ Conveying under any other circumstances is permitted solely under
+the conditions stated below. Sublicensing is not allowed; section 10
+makes it unnecessary.
+
+ 3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+ No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+ When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.
+
+ 4. Conveying Verbatim Copies.
+
+ You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+ You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+ 5. Conveying Modified Source Versions.
+
+ You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:
+
+ a) The work must carry prominent notices stating that you modified
+ it, and giving a relevant date.
+
+ b) The work must carry prominent notices stating that it is
+ released under this License and any conditions added under section
+ 7. This requirement modifies the requirement in section 4 to
+ "keep intact all notices".
+
+ c) You must license the entire work, as a whole, under this
+ License to anyone who comes into possession of a copy. This
+ License will therefore apply, along with any applicable section 7
+ additional terms, to the whole of the work, and all its parts,
+ regardless of how they are packaged. This License gives no
+ permission to license the work in any other way, but it does not
+ invalidate such permission if you have separately received it.
+
+ d) If the work has interactive user interfaces, each must display
+ Appropriate Legal Notices; however, if the Program has interactive
+ interfaces that do not display Appropriate Legal Notices, your
+ work need not make them do so.
+
+ A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+"aggregate" if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit. Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.
+
+ 6. Conveying Non-Source Forms.
+
+ You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:
+
+ a) Convey the object code in, or embodied in, a physical product
+ (including a physical distribution medium), accompanied by the
+ Corresponding Source fixed on a durable physical medium
+ customarily used for software interchange.
+
+ b) Convey the object code in, or embodied in, a physical product
+ (including a physical distribution medium), accompanied by a
+ written offer, valid for at least three years and valid for as
+ long as you offer spare parts or customer support for that product
+ model, to give anyone who possesses the object code either (1) a
+ copy of the Corresponding Source for all the software in the
+ product that is covered by this License, on a durable physical
+ medium customarily used for software interchange, for a price no
+ more than your reasonable cost of physically performing this
+ conveying of source, or (2) access to copy the
+ Corresponding Source from a network server at no charge.
+
+ c) Convey individual copies of the object code with a copy of the
+ written offer to provide the Corresponding Source. This
+ alternative is allowed only occasionally and noncommercially, and
+ only if you received the object code with such an offer, in accord
+ with subsection 6b.
+
+ d) Convey the object code by offering access from a designated
+ place (gratis or for a charge), and offer equivalent access to the
+ Corresponding Source in the same way through the same place at no
+ further charge. You need not require recipients to copy the
+ Corresponding Source along with the object code. If the place to
+ copy the object code is a network server, the Corresponding Source
+ may be on a different server (operated by you or a third party)
+ that supports equivalent copying facilities, provided you maintain
+ clear directions next to the object code saying where to find the
+ Corresponding Source. Regardless of what server hosts the
+ Corresponding Source, you remain obligated to ensure that it is
+ available for as long as needed to satisfy these requirements.
+
+ e) Convey the object code using peer-to-peer transmission, provided
+ you inform other peers where the object code and Corresponding
+ Source of the work are being offered to the general public at no
+ charge under subsection 6d.
+
+ A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.
+
+ A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling. In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage. For a particular
+product received by a particular user, "normally used" refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product. A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.
+
+ "Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source. The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.
+
+ If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information. But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+ The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed. Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.
+
+ Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+ 7. Additional Terms.
+
+ "Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law. If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.
+
+ When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it. (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.) You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+ Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:
+
+ a) Disclaiming warranty or limiting liability differently from the
+ terms of sections 15 and 16 of this License; or
+
+ b) Requiring preservation of specified reasonable legal notices or
+ author attributions in that material or in the Appropriate Legal
+ Notices displayed by works containing it; or
+
+ c) Prohibiting misrepresentation of the origin of that material, or
+ requiring that modified versions of such material be marked in
+ reasonable ways as different from the original version; or
+
+ d) Limiting the use for publicity purposes of names of licensors or
+ authors of the material; or
+
+ e) Declining to grant rights under trademark law for use of some
+ trade names, trademarks, or service marks; or
+
+ f) Requiring indemnification of licensors and authors of that
+ material by anyone who conveys the material (or modified versions of
+ it) with contractual assumptions of liability to the recipient, for
+ any liability that these contractual assumptions directly impose on
+ those licensors and authors.
+
+ All other non-permissive additional terms are considered "further
+restrictions" within the meaning of section 10. If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term. If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.
+
+ If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+ Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.
+
+ 8. Termination.
+
+ You may not propagate or modify a covered work except as expressly
+provided under this License. Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+ However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.
+
+ Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+ Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License. If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+ 9. Acceptance Not Required for Having Copies.
+
+ You are not required to accept this License in order to receive or
+run a copy of the Program. Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance. However,
+nothing other than this License grants you permission to propagate or
+modify any covered work. These actions infringe copyright if you do
+not accept this License. Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+ 10. Automatic Licensing of Downstream Recipients.
+
+ Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License. You are not responsible
+for enforcing compliance by third parties with this License.
+
+ An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations. If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+ You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License. For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+ 11. Patents.
+
+ A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based. The
+work thus licensed is called the contributor's "contributor version".
+
+ A contributor's "essential patent claims" are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version. For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+ Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+ In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement). To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+ If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients. "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+ If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+ A patent license is "discriminatory" if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License. You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.
+
+ Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+ 12. No Surrender of Others' Freedom.
+
+ If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License. If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all. For example, if you agree to terms that obligate you
+to collect a royalty for further conveying from those to whom you convey
+the Program, the only way you could satisfy both those terms and this
+License would be to refrain entirely from conveying the Program.
+
+ 13. Use with the GNU Affero General Public License.
+
+ Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU Affero General Public License into a single
+combined work, and to convey the resulting work. The terms of this
+License will continue to apply to the part which is the covered work,
+but the special requirements of the GNU Affero General Public License,
+section 13, concerning interaction through a network will apply to the
+combination as such.
+
+ 14. Revised Versions of this License.
+
+ The Free Software Foundation may publish revised and/or new versions of
+the GNU General Public License from time to time. Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+ Each version is given a distinguishing version number. If the
+Program specifies that a certain numbered version of the GNU General
+Public License "or any later version" applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation. If the Program does not specify a version number of the
+GNU General Public License, you may choose any version ever published
+by the Free Software Foundation.
+
+ If the Program specifies that a proxy can decide which future
+versions of the GNU General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+ Later license versions may give you additional or different
+permissions. However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+ 15. Disclaimer of Warranty.
+
+ THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+ 16. Limitation of Liability.
+
+ IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.
+
+ 17. Interpretation of Sections 15 and 16.
+
+ If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
diff --git a/README.md b/README.md
index 39d71593705405be0126418a087a434893f8c33b..8db7c470a0e95d47e2f01e033e8636ba77959a91 100644
--- a/README.md
+++ b/README.md
@@ -7,6 +7,11 @@ cd <your/path>
git clone <repo-link>
```
+Clone the RTKLIB lib as a sub-module:
+```
+git submodule update --init
+```
+
Create a directory to build all the source files:
```
$ mkdir build
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..c00bf50fd66b5e23484af5f8bcc75d5768425ecd
--- /dev/null
+++ b/include/gnss_utils/gnss_utils.h
@@ -0,0 +1,234 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef INCLUDE_GNSS_UTILS_GNSS_UTILS_H_
+#define INCLUDE_GNSS_UTILS_GNSS_UTILS_H_
+
+extern "C" {
+#include "rtklib.h"
+}
+
+// std includes
+#include <set>
+#include <map>
+#include <vector>
+#include <iostream>
+#include <memory>
+#include <cassert>
+// eigen
+#include <Eigen/Dense>
+
+#ifndef RAD2DEG
+#define RAD2DEG 180.0 / 3.14159265358979323846
+#endif
+
+#ifndef DEG2RAD
+#define DEG2RAD 3.14159265358979323846 / 180.0
+#endif
+
+namespace GnssUtils
+{
+
+// Structs
+struct ComputePosOutput
+{
+ time_t time;
+ double sec;
+ Eigen::Vector3d pos; // position (m)
+ Eigen::Vector3d vel; // velocity (m/s)
+ Eigen::Matrix3d pos_covar; // position covariance (m^2)
+ // {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx}
+ Eigen::VectorXd rcv_bias; // receiver clock bias to time systems (s) 0: receiver clock bias (s), 1: glo-gps time offset (s), 2: gal-gps time offset (s), 3: bds-gps time offset (s)
+ int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
+ int stat; // solution status (SOLQ_???)
+ int ns; // number of valid satellites
+ double age; // age of differential (s)
+ double ratio; // AR ratio factor for valiation
+
+ bool success; // return from pntpos true/false
+ std::string msg; // message returned (in case of error)
+ Eigen::Vector3d lat_lon; // latitude_longitude_altitude
+
+ std::map<int,Eigen::Vector2d> sat_azel; // azimuth and elevation of each satellite provided
+ std::set<int> used_sats; // used sats for computing fix (applying RAIM if the case and discarding wrong data)
+ std::set<int> discarded_sats; // discarded sats when computing fix (applying RAIM if the case and discarding wrong data)
+ std::map<int,double> prange_residuals; // residuals of used pseudoranges (applying RAIM if the case and discarding wrong data)
+};
+
+/* defaults processing options */
+const prcopt_t default_prcopt = {
+ PMODE_SINGLE, /* mode: positioning mode (PMODE_???) */
+ 0, /* soltype: solution type (0:forward,1:backward,2:combined) */
+ 2, /* nf: number of frequencies (1:L1,2:L1+L2,3:L1+L2+L3,4:L1+L2+L3+L4) */
+ SYS_GPS|SYS_GLO|SYS_GAL, /* navsys */
+ 15.0*D2R,{{0,0}}, /* elmin (rad) ,snrmask */
+ 0, /* satellite ephemeris option: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris */
+ 3,3,1,0,1, /* modear,glomodear,gpsmodear,bdsmodear,arfilter */
+ 20,0,4,5,10,20, /* maxout,minlock,minfixsats,minholdsats,mindropsats,minfix */
+ 0,1, /* rcvstds,armaxiter */
+ IONOOPT_BRDC, /* ionoopt: IONOOPT_OFF(0):correction off, IONOOPT_BRDC(1):broadcast mode, IONOOPT_SBAS(2):SBAS model, IONOOPT_IFLC(3):L1/L2 or L1/L5, IONOOPT_EST(4):estimation, IONOOPT_TEC(5):IONEX TEC model, IONOOPT_QZS(6):QZSS broadcast, IONOOPT_LEX(7):QZSS LEX ionosphere, IONOOPT_STEC(8):SLANT TEC mode */
+ TROPOPT_SAAS, /* tropopt: TROPOPT_OFF(0):correction off, TROPOPT_SAAS(1):Saastamoinen model, TROPOPT_SBAS(2):SBAS model, TROPOPT_EST(3):troposphere option: ZTD estimation, TROPOPT_ESTG(4):ZTD+grad estimation, TROPOPT_ZTD(5):ZTD correction,6:ZTD+grad correction */
+ 1,0, /* dynamics,tidecorr */
+ 3, /* niter: number of filter iteration */
+ 0,0, /* codesmooth,intpref */
+ 0, /* sbascorr: SBAS correction options (can be added): SBSOPT_LCORR(1): long term correction, SBSOPT_FCORR(2): fast correction, SBSOPT_ICORR(4): ionosphere correction, SBSOPT_RANGE(8): ranging */
+ 0, /* sbasssatsel: SBAS satellite selection (0:all) */
+ 0,0, /* rovpos,refpos */
+ WEIGHTOPT_ELEVATION, /* weightmode */
+ {300.0,300.0,300.0}, /* eratio[]: measurement error factor [0]:reserved [1-3]:error factor a/b/c of phase (m) [4]:doppler frequency (hz) [5]: snr max value (dB.Hz) */
+ {100.0,0.003,0.003,0.0,1.0,52.0}, /* err[]: */
+ {30.0,0.03,0.3}, /* std[] initial-state std [0]bias,[1]iono [2]trop */
+ {1E-4,1E-3,1E-4,1E-1,1E-2,0.0}, /* prn[] */
+ 5E-12, /* sclkstab */
+ {3.0,0.25,0.0,1E-9,1E-5,0.0,0.0,0.0}, /* thresar */
+ 0.0,0.0,0.05,0.1,0.01, /* elmaskar,elmaskhold,thresslip,varholdamb,gainholdamb */
+ 30.0, /* maxtdiff: max difference of time (sec) */
+ 5.0, /* maxinno: reject threshold of innovation (m) */
+ 30.0, /* maxgdop: reject threshold of gdop */
+ {0},{0},{0}, /* baseline,ru,rb */
+ {"",""}, /* anttype */
+ {{0}},{{0}},{0}, /* antdel,pcvr,exsats */
+ 1,1, /* maxaveep,initrst */
+ 0, /* outsingle */
+ {{0}}, /* rnxopt */
+ {0,0,0,0,1,0}, /* posopt: positioning options [0-1]:PPP pcv [2]:PPP phase windup [3]:PPP exclude eclipsing [4]:PNTPOS RAIM, [5]:PPP clock jump */
+ 0, /* syncsol */
+ {{0}}, /* odisp */
+ {{0}}, /* exterr */
+ 0, /* freqopt */
+ {0} /* pppopt */
+};
+
+struct TdcpOptions
+{
+ bool loss_function; // apply loss function in TDCP factors
+ double sigma_atm;
+ double sigma_carrier;
+ bool use_old_nav;
+ bool multi_freq;
+ double time_window; // window of time in which we perform TDCP
+
+ // Batch TDCP params
+ bool batch; // precompute global displacement between 2 epochs
+ int min_common_sats;
+ int raim_n;
+ bool validate_residual;
+ double max_residual_ci; // Confidence interval that defines the max residual to be considered good solution. RAIM applied if enabled in this case.
+ bool relinearize_jacobian;
+ int max_iterations;
+ int residual_opt; // 0: Normalized RMS of residual vector. 1: Max residual in Mahalanobis squared distance
+ int sagnac_correction; // 0 deactivated, 1/2 substraction/addition
+};
+
+struct CarrierPhaseOptions
+{
+ bool corr_iono;
+ bool corr_tropo;
+ bool corr_clock;
+};
+
+struct Options
+{
+ int sateph; // satellite ephemeris option: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris, EPHOPT_SBAS2(6):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), EPHOPT_SBAS3(7):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_SBAS2), EPHOPT_SBAS4(8):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_BRDC)
+ int ionoopt; // ionosphere option: IONOOPT_OFF(0):correction off, IONOOPT_BRDC(1):broadcast mode, IONOOPT_SBAS(2):SBAS model, IONOOPT_IFLC(3):L1/L2 or L1/L5, IONOOPT_EST(4):estimation, IONOOPT_TEC(5):IONEX TEC model, IONOOPT_QZS(6):QZSS broadcast, IONOOPT_LEX(7):QZSS LEX ionosphere, IONOOPT_STEC(8):SLANT TEC mode, IONOOPT_SBAS2(9):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), IONOOPT_SBAS3(10):broadcast + SBAS(IONOOPT_SBAS if possible, otherwise IONOOPT_SBAS2), IONOOPT_SBAS4(11):broadcast + SBAS(IONOOPT_SBAS if possible, otherwise IONOOPT_BRDC)
+ int tropopt; // troposphere option: TROPOPT_OFF(0):correction off, TROPOPT_SAAS(1):Saastamoinen model, TROPOPT_SBAS(2):SBAS model, TROPOPT_EST(3):troposphere option: ZTD estimation, TROPOPT_ESTG(4):ZTD+grad estimation, TROPOPT_ZTD(5):ZTD correction,6:ZTD+grad correction
+ int sbascorr; // SBAS correction options (can be added): SBSOPT_LCORR(1): long term correction, SBSOPT_FCORR(2): fast correction, SBSOPT_ICORR(4): ionosphere correction, SBSOPT_RANGE(8): ranging
+ int raim; // RAIM removed sats
+ double elmin; // min elevation (rad)
+ double maxgdop; // maxgdop: reject threshold of gdop
+ bool GPS,SBS,GLO,GAL,QZS,CMP,IRN,LEO; // constellations used
+ CarrierPhaseOptions carrier_opt; // Carrier phase correction options
+
+ // compute navsys int
+ int getNavSys() const
+ {
+ return GPS*SYS_GPS + SBS*SYS_SBS + GLO*SYS_GLO + GAL*SYS_GAL + QZS*SYS_QZS + CMP*SYS_CMP + IRN*SYS_IRN + LEO*SYS_LEO;
+ }
+
+ snrmask_t getSnrMask() const
+ {
+ return default_prcopt.snrmask;
+ }
+
+ // create a rtklib option struct from this
+ prcopt_t getPrcopt() const
+ {
+ prcopt_t opt{default_prcopt};
+ opt.sateph = sateph;
+ opt.ionoopt = ionoopt;
+ opt.tropopt = tropopt;
+ opt.sbascorr = sbascorr;
+ opt.posopt[4] = raim;
+ opt.elmin = elmin;
+ opt.maxgdop = maxgdop;
+ opt.navsys = getNavSys();
+ return opt;
+ }
+};
+
+const Options default_options =
+{
+ EPHOPT_BRDC, // satellite ephemeris option: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris, EPHOPT_SBAS2(6):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), EPHOPT_SBAS3(7):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_SBAS2), EPHOPT_SBAS4(8):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_BRDC)
+ IONOOPT_BRDC, // ionosphere option: IONOOPT_OFF(0):correction off, IONOOPT_BRDC(1):broadcast mode, IONOOPT_SBAS(2):SBAS model, IONOOPT_IFLC(3):L1/L2 or L1/L5, IONOOPT_EST(4):estimation, IONOOPT_TEC(5):IONEX TEC model, IONOOPT_QZS(6):QZSS broadcast, IONOOPT_LEX(7):QZSS LEX ionosphere, IONOOPT_STEC(8):SLANT TEC mode, IONOOPT_SBAS2(9):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), IONOOPT_SBAS3(10):broadcast + SBAS(IONOOPT_SBAS if possible, otherwise IONOOPT_SBAS2), IONOOPT_SBAS4(8):broadcast + SBAS(IONOOPT_SBAS if possible, otherwise IONOOPT_BRDC)
+ TROPOPT_SAAS, // troposphere option: TROPOPT_OFF(0):correction off, TROPOPT_SAAS(1):Saastamoinen model, TROPOPT_SBAS(2):SBAS model, TROPOPT_EST(3):troposphere option: ZTD estimation, TROPOPT_ESTG(4):ZTD+grad estimation, TROPOPT_ZTD(5):ZTD correction,6:ZTD+grad correction
+ 15, // SBAS correction options (can be added): SBSOPT_LCORR(1): long term correction, SBSOPT_FCORR(2): fast correction, SBSOPT_ICORR(4): ionosphere correction, SBSOPT_RANGE(8): ranging
+ 1, // RAIM enabled
+ D2R*15.0, // min elevation (degrees)
+ 30.0, // maxgdop: reject threshold of gdop
+ true, false, true, true, false, false, false, false, //GPS,SBS,GLO,GAL,QZS,CMP,IRN,LEO; // constellations used
+ {true, true, true} // carrier phase options
+};
+
+// forward declarations
+class Observations;
+class Navigation;
+class Snapshot;
+struct Satellite;
+class Range;
+
+// Typedefs
+typedef std::map<int,Satellite> Satellites;
+typedef std::map<int,Range> Ranges;
+typedef std::map<int,Eigen::Vector2d> Azels; // Azimuth and elevation (rad)
+
+// pointer typedefs
+typedef std::shared_ptr<Observations> ObservationsPtr;
+typedef std::shared_ptr<const Observations> ObservationsConstPtr;
+
+typedef std::shared_ptr<Navigation> NavigationPtr;
+typedef std::shared_ptr<const Navigation> NavigationConstPtr;
+
+typedef std::shared_ptr<Snapshot> SnapshotPtr;
+typedef std::shared_ptr<const Snapshot> SnapshotConstPtr;
+
+enum Combination
+{
+ CODE_L1, ///< only L1 code
+ CARRIER_L1, ///< only L1 carrier phase
+ CODE_IONO_FREE, ///< iono-free combination for code
+ CARRIER_IONO_FREE ///< iono-free combination for carrier phase
+};
+
+
+
+}
+
+#endif
diff --git a/include/gnss_utils/navigation.h b/include/gnss_utils/navigation.h
new file mode 100644
index 0000000000000000000000000000000000000000..ca4580b8fa9f61230477df71797f701c67ad0189
--- /dev/null
+++ b/include/gnss_utils/navigation.h
@@ -0,0 +1,251 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef INCLUDE_GNSS_UTILS_NAVIGATION_H_
+#define INCLUDE_GNSS_UTILS_NAVIGATION_H_
+
+#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/utils.h"
+
+namespace GnssUtils
+{
+
+class Navigation
+{
+public:
+ // Constructor & Destructor
+ Navigation();
+ Navigation(const Navigation& nav);
+ ~Navigation();
+
+ // Public objects
+
+ // Public methods
+ void setNavigation(nav_t nav);
+ void clearNavigation();
+ void
+ loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = "");
+
+ const nav_t& getNavigation() const;
+ nav_t& getNavigation();
+
+ void uniqueNavigation(); // remove duplicated ephemerides and update carrier phase wave lengths
+
+ /****************** Array memory management ******************/
+ bool addEphemeris(const eph_t& eph);
+ bool addGlonassEphemeris(const geph_t& geph);
+ bool addSbasEphemeris(const seph_t& seph);
+ bool addPreciseEphemeris(const peph_t& peph);
+ bool addPreciseClock(const pclk_t& pclk);
+ bool addAlmanac(const alm_t& alm);
+ bool addTec(const tec_t& tec);
+ bool addFcb(const fcbd_t& fcb);
+
+ void addSbasMessage(const sbsmsg_t& sbas_msg);
+
+ void copyAllArrays(const nav_t& nav);
+ void copyEphemeris(const nav_t& nav);
+ void copyAlmanac(const nav_t& nav);
+ void copyIonUtc(const nav_t& nav);
+ void copySbasCorrections(const nav_t& nav);
+ void copyBias(const nav_t& nav);
+
+ void freeNavigationArrays();
+ void freeEphemeris();
+ void freeGlonassEphemeris();
+ void freeSbasEphemeris();
+ void freePreciseEphemeris();
+ void freeAlmanac();
+ void freePreciseClock();
+ void freeTecData();
+ void freeFcbData();
+
+ void print();
+
+ //////////////////////////////// nav UTILS //////////////////////////////////////
+ static void freeEph(nav_t& nav);
+ static void freeGeph(nav_t& nav);
+ static void freeSeph(nav_t& nav);
+ static void freePeph(nav_t& nav);
+ static void freeAlm(nav_t& nav);
+ static void freePclk(nav_t& nav);
+ static void freeTec(nav_t& nav);
+ static void freeFcb(nav_t& nav);
+ static void freeNavArrays(nav_t& nav);
+
+private:
+ // rtklib-like attribute to represent the different navigation msgs for a given epoch
+ nav_t nav_;
+
+ // Private methods
+};
+
+inline void Navigation::uniqueNavigation() // remove duplicated ephemerides and update wave lengths
+{
+ uniqnav(&nav_);
+}
+
+inline const nav_t& Navigation::getNavigation() const
+{
+ return nav_;
+}
+
+inline nav_t& Navigation::getNavigation()
+{
+ return nav_;
+}
+
+inline bool Navigation::addEphemeris(const eph_t& eph)
+{
+ return addToArray<eph_t>(eph, nav_.eph, nav_.n, nav_.nmax);
+}
+
+inline bool Navigation::addGlonassEphemeris(const geph_t& geph)
+{
+ return addToArray<geph_t>(geph, nav_.geph, nav_.ng, nav_.ngmax);
+}
+
+inline bool Navigation::addSbasEphemeris(const seph_t& seph)
+{
+ return addToArray<seph_t>(seph, nav_.seph, nav_.ns, nav_.nsmax);
+}
+
+inline bool Navigation::addPreciseEphemeris(const peph_t& peph)
+{
+ return addToArray<peph_t>(peph, nav_.peph, nav_.ne, nav_.nemax);
+}
+
+inline bool Navigation::addPreciseClock(const pclk_t& pclk)
+{
+ return addToArray<pclk_t>(pclk, nav_.pclk, nav_.nc, nav_.ncmax);
+}
+
+inline bool Navigation::addAlmanac(const alm_t& alm)
+{
+ return addToArray<alm_t>(alm, nav_.alm, nav_.na, nav_.namax);
+}
+
+inline bool Navigation::addTec(const tec_t& tec)
+{
+ return addToArray<tec_t>(tec, nav_.tec, nav_.nt, nav_.ntmax);
+}
+
+inline bool Navigation::addFcb(const fcbd_t& fcb)
+{
+ return addToArray<fcbd_t>(fcb, nav_.fcb, nav_.nf, nav_.nfmax);
+}
+
+inline void Navigation::freeEphemeris()
+{
+ freeEph(nav_);
+}
+
+inline void Navigation::freeGlonassEphemeris()
+{
+ freeGeph(nav_);
+}
+
+inline void Navigation::freePreciseEphemeris()
+{
+ freePeph(nav_);
+}
+
+inline void Navigation::freeSbasEphemeris()
+{
+ freeSeph(nav_);
+}
+
+inline void Navigation::freePreciseClock()
+{
+ freePclk(nav_);
+}
+
+inline void Navigation::freeTecData()
+{
+ freeTec(nav_);
+}
+
+inline void Navigation::freeFcbData()
+{
+ freeFcb(nav_);
+}
+
+inline void Navigation::freeAlmanac()
+{
+ freeAlm(nav_);
+}
+
+//////////////////////////////// nav UTILS //////////////////////////////////////
+inline void Navigation::freeEph(nav_t& nav)
+{
+ freeArray<eph_t>(nav.eph, nav.n, nav.nmax);
+}
+
+inline void Navigation::freeGeph(nav_t& nav)
+{
+ freeArray<geph_t>(nav.geph, nav.ng, nav.ngmax);
+}
+
+inline void Navigation::freeSeph(nav_t& nav)
+{
+ freeArray<seph_t>(nav.seph, nav.ns, nav.nsmax);
+}
+
+inline void Navigation::freePeph(nav_t& nav)
+{
+ freeArray<peph_t>(nav.peph, nav.ne, nav.nemax);
+}
+
+inline void Navigation::freeAlm(nav_t& nav)
+{
+ freeArray<alm_t>(nav.alm, nav.na, nav.namax);
+}
+
+inline void Navigation::freePclk(nav_t& nav)
+{
+ freeArray<pclk_t>(nav.pclk, nav.nc, nav.ncmax);
+}
+
+inline void Navigation::freeTec(nav_t& nav)
+{
+ freeArray<tec_t>(nav.tec, nav.nt, nav.ntmax);
+}
+
+inline void Navigation::freeFcb(nav_t& nav)
+{
+ freeArray<fcbd_t>(nav.fcb, nav.nf, nav.nfmax);
+}
+
+inline void Navigation::freeNavArrays(nav_t& nav)
+{
+ // RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
+ freeEph(nav);
+ freeGeph(nav);
+ freeSeph(nav);
+ freePeph(nav);
+ freePclk(nav);
+ freeAlm(nav);
+ freeTec(nav);
+ freeFcb(nav);
+}
+
+} // namespace GnssUtils
+#endif // INCLUDE_GNSS_UTILS_NAVIGATION_H_
diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
new file mode 100644
index 0000000000000000000000000000000000000000..52b66bff06e0870ecd968b453feb4bdf6a98a116
--- /dev/null
+++ b/include/gnss_utils/observations.h
@@ -0,0 +1,182 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef INCLUDE_GNSS_UTILS_OBSERVATIONS_H_
+#define INCLUDE_GNSS_UTILS_OBSERVATIONS_H_
+
+#include "gnss_utils/gnss_utils.h"
+
+namespace GnssUtils
+{
+
+class Observations
+{
+public:
+ // Constructor & Destructor
+ Observations();
+ Observations(const Observations& obs);
+ ~Observations();
+
+ void clearObservations();
+ void addObservation(const obsd_t& obs);
+ void removeObservationByIdx(const int& _idx);
+ void removeObservationBySat(const int& _sat);
+ std::vector<obsd_t>& getObservations();
+ const std::vector<obsd_t>& getObservations() const;
+ obsd_t& getObservationBySat(const unsigned char& sat_number);
+ const obsd_t& getObservationBySat(const unsigned char& sat_number) const;
+ obsd_t& getObservationByIdx(const int& idx);
+ const obsd_t& getObservationByIdx(const int& idx) const;
+
+ obsd_t* data();
+ const obsd_t* data() const;
+ size_t size() const;
+
+ bool hasSatellite(const unsigned char& i) const;
+
+ static void print(obsd_t& _obs);
+ void printBySat(const int& _sat);
+ void printByIdx(const int& _idx);
+ void print();
+
+ // Filter observations
+ std::set<int> filterByEphemeris(const Satellites& sats);
+ std::set<int> filterBySatellites(const std::set<int>& discarded_sats);
+ std::set<int> filterByCode();
+ std::set<int> filterByCarrierPhase();
+ std::set<int> filterByConstellations(const int& navsys);
+ std::set<int> filterByElevationSnr(const Azels& azels,
+ const snrmask_t& snrmask,
+ const double& elmin,
+ const bool& multi_freq);
+ std::set<int> filter(const Satellites& sats,
+ const std::set<int>& discarded_sats,
+ const Eigen::Vector3d& x_r,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const Options& opt);
+ std::set<int> filter(const Satellites& sats,
+ const std::set<int>& discarded_sats,
+ const Eigen::Vector3d& x_r,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const int& navsys,
+ const snrmask_t& snrmask,
+ const double& elmin,
+ const bool& multi_freq);
+ std::set<int> filter(const Satellites& sats,
+ const std::set<int>& discarded_sats,
+ const Azels& azels,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const Options& opt);
+ std::set<int> filter(const Satellites& sats,
+ const std::set<int>& discarded_sats,
+ const Azels& azels,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const int& navsys,
+ const snrmask_t& snrmask,
+ const double& elmin,
+ const bool& multi_freq);
+
+ // Others
+ static std::set<int> findCommonObservations(const Observations& obs_1, const Observations& obs_2);
+
+ bool operator==(const Observations& other_obs) const;
+ bool operator!=(const Observations& other_obs) const;
+
+private:
+ // Private objects
+ std::map<unsigned char, int> sat_2_idx_; //< key: corresponding sat number, value: idx in obs_ vector
+ std::vector<unsigned char> idx_2_sat_; //< key: idx in obs_ vector, value: corresponding sat number
+ std::vector<obsd_t> obs_; //< vector of RTKLIB observations for a given epoch
+};
+
+} // namespace GnssUtils
+
+// IMPLEMENTATION
+#include "gnss_utils/utils/utils.h"
+#include "gnss_utils/utils/satellite.h"
+
+namespace GnssUtils
+{
+inline const std::vector<obsd_t>& Observations::getObservations() const
+{
+ return this->obs_;
+}
+
+inline std::vector<obsd_t>& Observations::getObservations()
+{
+ return this->obs_;
+}
+
+inline obsd_t& Observations::getObservationBySat(const unsigned char& sat_number)
+{
+ assert(sat_2_idx_.count(sat_number) != 0);
+ return obs_.at(sat_2_idx_.at(sat_number));
+}
+
+inline const obsd_t& Observations::getObservationBySat(const unsigned char& sat_number) const
+{
+ assert(sat_2_idx_.count(sat_number) != 0);
+ return obs_.at(sat_2_idx_.at(sat_number));
+}
+
+inline obsd_t& Observations::getObservationByIdx(const int& idx)
+{
+ assert(obs_.size() > idx && idx >= 0);
+ return obs_.at(idx);
+}
+
+inline const obsd_t& Observations::getObservationByIdx(const int& idx) const
+{
+ assert(obs_.size() > idx && idx >= 0);
+ return obs_.at(idx);
+}
+
+inline obsd_t* Observations::data()
+{
+ return obs_.data();
+}
+
+inline const obsd_t* Observations::data() const
+{
+ return obs_.data();
+}
+
+inline size_t Observations::size() const
+{
+ return obs_.size();
+}
+
+inline bool Observations::hasSatellite(const unsigned char& i) const
+{
+ return sat_2_idx_.count(i) != 0;
+}
+
+inline bool Observations::operator!=(const Observations& other_obs) const
+{
+ return !(*this == other_obs);
+}
+
+} // namespace GnssUtils
+#endif // INCLUDE_GNSS_UTILS_OBSERVATIONS_H_
diff --git a/include/gnss_utils/range.h b/include/gnss_utils/range.h
new file mode 100644
index 0000000000000000000000000000000000000000..d783793f6024859988bd867caab49ac6c274264f
--- /dev/null
+++ b/include/gnss_utils/range.h
@@ -0,0 +1,75 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * range.h
+ *
+ * Created on: May 28, 2020
+ * Author: joanvallve
+ */
+
+#ifndef INCLUDE_GNSS_UTILS_RANGE_H_
+#define INCLUDE_GNSS_UTILS_RANGE_H_
+
+#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+
+namespace GnssUtils
+{
+
+class Range
+{
+ public:
+ int sys = SYS_GPS;
+ int sat = 0;
+ double P = -1;
+ double P_var = 1;
+ double P_corrected = -1;
+ double iono_corr = 0;
+ double tropo_corr = 0;
+ double sat_clock_corr = 0;
+ double L = -1;
+ double L_corrected = -1;
+ double L_var = 1;
+ double L2 = -1;
+ double L2_corrected = -1;
+ double L2_var = 1;
+
+ public:
+ Range();
+ virtual ~Range();
+
+ static Ranges computeRanges(ObservationsPtr obs,
+ NavigationPtr nav,
+ const Satellites& sats,
+ const Azels& azel,
+ const Eigen::Vector3d& latlonalt,
+ const Options& opt);
+
+ static std::set<int> findCommonSatellites(const Ranges& ranges_1, const Ranges& ranges_2);
+};
+
+double computeSagnacCorrection(const Eigen::Vector3d& rcv_ECEF, const Eigen::Vector3d& sat_ECEF);
+
+} /* namespace GnssUtils */
+
+#endif /* INCLUDE_GNSS_UTILS_RANGE_H_ */
diff --git a/include/gnss_utils/receiver_raw_base.h b/include/gnss_utils/receiver_raw_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..01161ed0747bea499e5490521da407e231b90c61
--- /dev/null
+++ b/include/gnss_utils/receiver_raw_base.h
@@ -0,0 +1,101 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef INCLUDE_GNSS_UTILS_RECEIVER_RAW_BASE_H_
+#define INCLUDE_GNSS_UTILS_RECEIVER_RAW_BASE_H_
+
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+
+namespace GnssUtils
+{
+enum RawDataType : int
+{
+ NO = 0,
+ OBS = 1,
+ NAV_EPH = 2,
+ NAV_SBAS = 3,
+ NAV_ALM_IONUTC = 9,
+ NAV_ANT = 5,
+ NAV_DGPS = 7,
+ NAV_SSR = 10,
+ NAV_LEX = 31,
+ ERROR = -1
+};
+
+// pointer typedefs
+class ReceiverRawAbstract;
+typedef std::shared_ptr<ReceiverRawAbstract> ReceiverRawAbstractPtr;
+typedef std::shared_ptr<const ReceiverRawAbstract> ReceiverRawAbstractConstPtr;
+
+class ReceiverRawAbstract
+{
+public:
+ ReceiverRawAbstract();
+ ~ReceiverRawAbstract();
+
+ virtual RawDataType addDataStream(const std::vector<uint8_t>& data_stream) = 0;
+
+ const Observations& getObservations() const;
+ const Navigation& getNavigation() const;
+
+ Observations getObservations();
+ Navigation getNavigation();
+
+ RawDataType getRawDataType() const;
+
+protected:
+ void updateObservations();
+
+ raw_t raw_data_;
+ RawDataType raw_data_type_;
+
+ Observations obs_;
+ Navigation nav_;
+};
+
+inline const Observations& ReceiverRawAbstract::getObservations() const
+{
+ return obs_;
+}
+
+inline Observations ReceiverRawAbstract::getObservations()
+{
+ return obs_;
+}
+
+inline const Navigation& ReceiverRawAbstract::getNavigation() const
+{
+ return nav_;
+}
+
+inline Navigation ReceiverRawAbstract::getNavigation()
+{
+ return nav_;
+}
+
+inline RawDataType ReceiverRawAbstract::getRawDataType() const
+{
+ return raw_data_type_;
+}
+
+} // namespace GnssUtils
+#endif
diff --git a/include/gnss_utils/receivers/novatel_raw.h b/include/gnss_utils/receivers/novatel_raw.h
new file mode 100644
index 0000000000000000000000000000000000000000..d27680351c47f5d937631d7ad9d625f7219df89f
--- /dev/null
+++ b/include/gnss_utils/receivers/novatel_raw.h
@@ -0,0 +1,39 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef INCLUDE_GNSS_UTILS_NOVATEL_RAW_H_
+#define INCLUDE_GNSS_UTILS_NOVATEL_RAW_H_
+
+#include "gnss_utils/receiver_raw_base.h"
+
+namespace GnssUtils
+{
+class NovatelRaw : public ReceiverRawAbstract
+{
+public:
+ NovatelRaw();
+ ~NovatelRaw();
+
+ RawDataType addDataStream(const std::vector<uint8_t>& data_stream) override;
+};
+} // namespace GnssUtils
+
+#endif
diff --git a/include/gnss_utils/receivers/ublox_raw.h b/include/gnss_utils/receivers/ublox_raw.h
new file mode 100644
index 0000000000000000000000000000000000000000..7cf92bbceba357f5af314bc72ed321cdf0e6d59e
--- /dev/null
+++ b/include/gnss_utils/receivers/ublox_raw.h
@@ -0,0 +1,43 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef INCLUDE_GNSS_UTILS_UBLOX_RAW_H_
+#define INCLUDE_GNSS_UTILS_UBLOX_RAW_H_
+
+#include "gnss_utils/receiver_raw_base.h"
+
+namespace GnssUtils
+{
+class UBloxRaw : public ReceiverRawAbstract
+{
+public:
+ UBloxRaw();
+ ~UBloxRaw();
+
+ RawDataType addDataStream(const std::vector<uint8_t>& data_stream) override;
+
+private:
+
+};
+
+} // namespace GnssUtils
+
+#endif // INCLUDE_GNSS_UTILS_UBLOX_RAW_H_
diff --git a/include/gnss_utils/snapshot.h b/include/gnss_utils/snapshot.h
new file mode 100644
index 0000000000000000000000000000000000000000..2381038cfd24efd7151bcbeca8017badc12e24dc
--- /dev/null
+++ b/include/gnss_utils/snapshot.h
@@ -0,0 +1,201 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef INCLUDE_GNSS_UTILS_SNAPSHOT_H_
+#define INCLUDE_GNSS_UTILS_SNAPSHOT_H_
+
+#include <map>
+#include <set>
+#include <iostream>
+#include <memory>
+#include <cassert>
+
+#include <Eigen/Dense>
+
+#include "gnss_utils/gnss_utils.h"
+
+namespace GnssUtils
+{
+
+class Snapshot
+{
+
+public:
+ // Constructor & Destructor
+ Snapshot();
+ Snapshot(ObservationsPtr obs, NavigationPtr nav);
+ ~Snapshot(){};
+
+ // Public objects
+
+ // Public methods
+ void loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = "");
+ double getGPST() const;
+
+ // Navigation
+ NavigationPtr getNavigation() const;
+ void setNavigation(NavigationPtr nav);
+
+ // Observations
+ ObservationsPtr getObservations() const;
+ void setObservations(ObservationsPtr obs);
+
+ std::set<int> filterObservations(const std::set<int>& discarded_sats,
+ const Eigen::Vector3d& x_r,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const Options& opt);
+ std::set<int> filterObservations(const std::set<int>& discarded_sats,
+ const Azels& azels,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const Options& opt);
+
+ // Satellites
+ const Satellites& getSatellites() const;
+ Satellites& getSatellites();
+ void computeSatellites(const int& eph_opt); // see rtklib.h L396);
+ bool satellitesComputed() const;
+
+ // Ranges
+ Ranges& getRanges();
+ const Ranges& getRanges() const;
+ bool rangesComputed() const;
+ void computeRanges(const Azels& azel,
+ const Eigen::Vector3d& latlonalt,
+ const Options& opt);
+ void computeRanges(const Eigen::Vector3d& x_ecef,
+ const Options& opt);
+ void print();
+
+private:
+ // Private objects
+ Satellites sats_;
+ ObservationsPtr obs_;
+ NavigationPtr nav_;
+ Ranges ranges_;
+
+ // Private methods
+};
+
+} // namespace GnssUtils
+
+// IMPLEMENTATION
+#include "gnss_utils/utils/satellite.h"
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+#include "gnss_utils/utils/utils.h"
+#include "gnss_utils/range.h"
+
+namespace GnssUtils
+{
+
+inline double Snapshot::getGPST() const
+{
+ return obs_->getObservations()[0].time.time + obs_->getObservations()[0].time.sec;
+}
+
+inline GnssUtils::ObservationsPtr Snapshot::getObservations() const
+{
+ return obs_;
+}
+
+inline GnssUtils::NavigationPtr Snapshot::getNavigation() const
+{
+ return nav_;
+}
+
+inline Satellites& Snapshot::getSatellites()
+{
+ return sats_;
+}
+
+inline const Satellites& Snapshot::getSatellites() const
+{
+ return sats_;
+}
+
+inline void Snapshot::setObservations(ObservationsPtr obs)
+{
+ obs_ = obs;
+}
+
+inline void Snapshot::setNavigation(NavigationPtr nav)
+{
+ nav_ = nav;
+}
+
+inline bool Snapshot::satellitesComputed() const
+{
+ return !sats_.empty();
+}
+
+inline std::set<int> Snapshot::filterObservations(const std::set<int>& discarded_sats,
+ const Eigen::Vector3d& x_r,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const Options& opt)
+{
+ std::set<int> filtered_sats = obs_->filter(sats_, discarded_sats, x_r, check_code, check_carrier_phase, opt);
+
+ for (auto sat : filtered_sats)
+ {
+ sats_.erase(sat);
+ ranges_.erase(sat);
+ }
+
+ return filtered_sats;
+}
+
+inline std::set<int> Snapshot::filterObservations(const std::set<int>& discarded_sats,
+ const Azels& azels,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const Options& opt)
+{
+ std::set<int> filtered_sats = obs_->filter(sats_, discarded_sats, azels, check_code, check_carrier_phase, opt);
+
+ for (auto sat : filtered_sats)
+ {
+ sats_.erase(sat);
+ ranges_.erase(sat);
+ }
+
+ return filtered_sats;
+}
+
+inline const Ranges& Snapshot::getRanges() const
+{
+ return ranges_;
+}
+
+inline Ranges& Snapshot::getRanges()
+{
+ return ranges_;
+}
+
+inline bool Snapshot::rangesComputed() const
+{
+ return !ranges_.empty();
+}
+
+} // namespace GnssUtils
+#endif // INCLUDE_GNSS_UTILS_SNAPSHOT_H_
diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
new file mode 100644
index 0000000000000000000000000000000000000000..04a5748455e57ed1b018bd1df678b0ec47f983c4
--- /dev/null
+++ b/include/gnss_utils/tdcp.h
@@ -0,0 +1,90 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef INCLUDE_GNSS_UTILS_TDCP_H_
+#define INCLUDE_GNSS_UTILS_TDCP_H_
+
+#define GNSS_UTILS_TDCP_DEBUG 0
+
+#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/chisquare_ci.h"
+
+namespace GnssUtils
+{
+
+struct TdcpOutput
+{
+ bool success = false;
+ std::string msg = "";
+ std::set<int> raim_discarded_sats;
+ std::set<int> used_sats;
+ Eigen::Vector4d d = Eigen::Vector4d::Zero();
+ Eigen::Matrix4d cov_d = Eigen::Matrix4d::Zero();
+ double dt = 0;
+ double residual = 0;
+ double residual_ci = 0;
+};
+
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector4d& d_0,
+ const std::set<int>& discarded_sats,
+ const TdcpOptions& tdcp_params,
+ const Options& opt);
+
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector3d& x_r,
+ const Eigen::Vector4d& d_0,
+ const std::set<int>& discarded_sats,
+ const TdcpOptions& tdcp_params,
+ const Options& opt);
+
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector3d& x_r,
+ const Eigen::Vector4d& d_0,
+ const std::set<int>& discarded_sats,
+ const std::set<int>& tracked_sats,
+ const TdcpOptions& tdcp_params,
+ const Options& opt);
+
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector3d& x_r,
+ const std::set<int>& common_sats,
+ const Eigen::Vector4d& d_0,
+ const TdcpOptions& tdcp_params);
+
+TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
+ Eigen::MatrixXd& A,
+ Eigen::VectorXd& r,
+ Eigen::VectorXd& drho_m,
+ const Eigen::MatrixXd& s_k,
+ const Eigen::MatrixXd& s_r,
+ const Eigen::Vector4d& d_0,
+ const double& var_tdcp,
+ std::set<int>& raim_discarded_rows,
+ const TdcpOptions& tdcp_params);
+
+} // namespace GnssUtils
+
+#endif /* INCLUDE_GNSS_UTILS_Tdcp_H_ */
diff --git a/include/gnss_utils/utils/chisquare_ci.h b/include/gnss_utils/utils/chisquare_ci.h
new file mode 100644
index 0000000000000000000000000000000000000000..29e2d44d4823a5bffa78411cb2ce65949dcd1622
--- /dev/null
+++ b/include/gnss_utils/utils/chisquare_ci.h
@@ -0,0 +1,79 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * chisquare_ci.h
+ *
+ * Created on: Jul 28, 2021
+ * Author: joanvallve
+ */
+
+#ifndef INCLUDE_GNSS_UTILS_UTILS_CHISQUARE_CI_H_
+#define INCLUDE_GNSS_UTILS_UTILS_CHISQUARE_CI_H_
+
+#include "chisquare_ci_maps.h"
+
+namespace GnssUtils
+{
+
+inline double chisq2ci(double chisq, int dof)
+{
+ assert(dof > 0);
+
+ if (dof > 30)
+ dof = 30;
+
+ if (chisq_2_CI.at(dof).count(chisq) == 1)
+ return chisq_2_CI.at(dof).at(chisq);
+
+ auto upper = chisq_2_CI.at(dof).upper_bound(chisq);
+
+ if (upper == chisq_2_CI.at(dof).begin())
+ return upper->second;
+
+ auto lower = std::prev(upper);
+
+ return lower->second + (chisq - lower->first) / (upper->first - lower->first) * (upper->second - lower->second);
+};
+
+inline double ci2chisq(double ci, int dof)
+{
+ assert(dof > 0);
+
+ if (dof > 30)
+ dof = 30;
+
+ if (CI_2_chisq.at(dof).count(ci) == 1)
+ return CI_2_chisq.at(dof).at(ci);
+
+ auto upper = CI_2_chisq.at(dof).upper_bound(ci);
+
+ if (upper == CI_2_chisq.at(dof).begin())
+ return upper->second;
+
+ auto lower = std::prev(upper);
+
+ return lower->second + (ci - lower->first) / (upper->first - lower->first) * (upper->second - lower->second);
+};
+
+} // namespace GnssUtils
+
+#endif /* INCLUDE_GNSS_UTILS_UTILS_CHISQUARE_CI_H_ */
diff --git a/include/gnss_utils/utils/chisquare_ci_maps.h b/include/gnss_utils/utils/chisquare_ci_maps.h
new file mode 100644
index 0000000000000000000000000000000000000000..c3413ff8ea5f29c0e089f39c9b5bedc0b8de58e3
--- /dev/null
+++ b/include/gnss_utils/utils/chisquare_ci_maps.h
@@ -0,0 +1,1418 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef INCLUDE_GNSS_UTILS_UTILS_CHISQUARE_CI_MAPS_H_
+#define INCLUDE_GNSS_UTILS_UTILS_CHISQUARE_CI_MAPS_H_
+
+#include <map>
+
+namespace GnssUtils
+{
+
+static std::map<int, std::map<double, double>> CI_2_chisq = {
+ // 1 dof
+ {1, {
+ {0.10, 0.0157908},
+ {0.15, 0.0357658},
+ {0.20, 0.0641848},
+ {0.25, 0.101531},
+ {0.30, 0.148472},
+ {0.35, 0.2059},
+ {0.40, 0.274996},
+ {0.45, 0.357317},
+ {0.50, 0.454936},
+ {0.55, 0.570652},
+ {0.60, 0.708326},
+ {0.65, 0.873457},
+ {0.70, 1.07419},
+ {0.75, 1.3233},
+ {0.80, 1.64237},
+ {0.85, 2.07225},
+ {0.90, 2.70554},
+ {0.95, 3.84146},
+ {0.99, 6.6349},
+ {1.00, 1e+12},
+ }},
+ // 2 dof
+ {2, {
+ {0.10, 0.210721},
+ {0.15, 0.325038},
+ {0.20, 0.446287},
+ {0.25, 0.575364},
+ {0.30, 0.71335},
+ {0.35, 0.861566},
+ {0.40, 1.02165},
+ {0.45, 1.19567},
+ {0.50, 1.38629},
+ {0.55, 1.59702},
+ {0.60, 1.83258},
+ {0.65, 2.09964},
+ {0.70, 2.40795},
+ {0.75, 2.77259},
+ {0.80, 3.21888},
+ {0.85, 3.79424},
+ {0.90, 4.60517},
+ {0.95, 5.99146},
+ {0.99, 9.21034},
+ {1.00, 1e+12},
+ }},
+ // 3 dof
+ {3, {
+ {0.10, 0.584374},
+ {0.15, 0.797771},
+ {0.20, 1.00517},
+ {0.25, 1.21253},
+ {0.30, 1.42365},
+ {0.35, 1.64158},
+ {0.40, 1.86917},
+ {0.45, 2.10947},
+ {0.50, 2.36597},
+ {0.55, 2.64301},
+ {0.60, 2.94617},
+ {0.65, 3.28311},
+ {0.70, 3.66487},
+ {0.75, 4.10834},
+ {0.80, 4.64163},
+ {0.85, 5.31705},
+ {0.90, 6.25139},
+ {0.95, 7.81473},
+ {0.99, 11.3449},
+ {1.00, 1e+12},
+ }},
+ // 4 dof
+ {4, {
+ {0.10, 1.06362},
+ {0.15, 1.36648},
+ {0.20, 1.64878},
+ {0.25, 1.92256},
+ {0.30, 2.1947},
+ {0.35, 2.47009},
+ {0.40, 2.75284},
+ {0.45, 3.04695},
+ {0.50, 3.35669},
+ {0.55, 3.68713},
+ {0.60, 4.04463},
+ {0.65, 4.43769},
+ {0.70, 4.87843},
+ {0.75, 5.38527},
+ {0.80, 5.98862},
+ {0.85, 6.74488},
+ {0.90, 7.77944},
+ {0.95, 9.48773},
+ {0.99, 13.2767},
+ {1.00, 1e+12},
+ }},
+ // 5 dof
+ {5, {
+ {0.10, 1.61031},
+ {0.15, 1.99382},
+ {0.20, 2.34253},
+ {0.25, 2.6746},
+ {0.30, 2.99991},
+ {0.35, 3.32511},
+ {0.40, 3.6555},
+ {0.45, 3.99594},
+ {0.50, 4.35146},
+ {0.55, 4.72776},
+ {0.60, 5.13187},
+ {0.65, 5.57307},
+ {0.70, 6.06443},
+ {0.75, 6.62568},
+ {0.80, 7.28928},
+ {0.85, 8.1152},
+ {0.90, 9.23636},
+ {0.95, 11.0705},
+ {0.99, 15.0863},
+ {1.00, 1e+12},
+ }},
+ // 6 dof
+ {6, {
+ {0.10, 2.20413},
+ {0.15, 2.66127},
+ {0.20, 3.07009},
+ {0.25, 3.4546},
+ {0.30, 3.82755},
+ {0.35, 4.19727},
+ {0.40, 4.57015},
+ {0.45, 4.95188},
+ {0.50, 5.34812},
+ {0.55, 5.7652},
+ {0.60, 6.21076},
+ {0.65, 6.69476},
+ {0.70, 7.23114},
+ {0.75, 7.8408},
+ {0.80, 8.55806},
+ {0.85, 9.4461},
+ {0.90, 10.6446},
+ {0.95, 12.5916},
+ {0.99, 16.8119},
+ {1.00, 1e+12},
+ }},
+ // 7 dof
+ {7, {
+ {0.10, 2.83311},
+ {0.15, 3.35828},
+ {0.20, 3.82232},
+ {0.25, 4.25485},
+ {0.30, 4.67133},
+ {0.35, 5.08165},
+ {0.40, 5.49323},
+ {0.45, 5.91252},
+ {0.50, 6.34581},
+ {0.55, 6.79997},
+ {0.60, 7.28321},
+ {0.65, 7.80612},
+ {0.70, 8.38343},
+ {0.75, 9.03715},
+ {0.80, 9.80325},
+ {0.85, 10.7479},
+ {0.90, 12.017},
+ {0.95, 14.0671},
+ {0.99, 18.4753},
+ {1.00, 1e+12},
+ }},
+ // 8 dof
+ {8, {
+ {0.10, 3.48954},
+ {0.15, 4.0782},
+ {0.20, 4.59357},
+ {0.25, 5.07064},
+ {0.30, 5.52742},
+ {0.35, 5.97529},
+ {0.40, 6.42265},
+ {0.45, 6.87663},
+ {0.50, 7.34412},
+ {0.55, 7.83251},
+ {0.60, 8.35053},
+ {0.65, 8.90936},
+ {0.70, 9.52446},
+ {0.75, 10.2189},
+ {0.80, 11.0301},
+ {0.85, 12.0271},
+ {0.90, 13.3616},
+ {0.95, 15.5073},
+ {0.99, 20.0902},
+ {1.00, 1e+12},
+ }},
+ // 9 dof
+ {9, {
+ {0.10, 4.16816},
+ {0.15, 4.81652},
+ {0.20, 5.38005},
+ {0.25, 5.89883},
+ {0.30, 6.39331},
+ {0.35, 6.87627},
+ {0.40, 7.35703},
+ {0.45, 7.84342},
+ {0.50, 8.34283},
+ {0.55, 8.86317},
+ {0.60, 9.41364},
+ {0.65, 10.006},
+ {0.70, 10.6564},
+ {0.75, 11.3888},
+ {0.80, 12.2421},
+ {0.85, 13.288},
+ {0.90, 14.6837},
+ {0.95, 16.919},
+ {0.99, 21.666},
+ {1.00, 1e+12},
+ }},
+ // 10 dof
+ {10, {
+ {0.10, 4.86518},
+ {0.15, 5.57006},
+ {0.20, 6.17908},
+ {0.25, 6.7372},
+ {0.30, 7.26722},
+ {0.35, 7.78324},
+ {0.40, 8.29547},
+ {0.45, 8.81235},
+ {0.50, 9.34182},
+ {0.55, 9.89222},
+ {0.60, 10.4732},
+ {0.65, 11.0971},
+ {0.70, 11.7807},
+ {0.75, 12.5489},
+ {0.80, 13.442},
+ {0.85, 14.5339},
+ {0.90, 15.9872},
+ {0.95, 18.307},
+ {0.99, 23.2093},
+ {1.00, 1e+12},
+ }},
+ // 11 dof
+ {11, {
+ {0.10, 5.57778},
+ {0.15, 6.33643},
+ {0.20, 6.98867},
+ {0.25, 7.58414},
+ {0.30, 8.14787},
+ {0.35, 8.69524},
+ {0.40, 9.23729},
+ {0.45, 9.78306},
+ {0.50, 10.341},
+ {0.55, 10.9199},
+ {0.60, 11.5298},
+ {0.65, 12.1836},
+ {0.70, 12.8987},
+ {0.75, 13.7007},
+ {0.80, 14.6314},
+ {0.85, 15.7671},
+ {0.90, 17.275},
+ {0.95, 19.6751},
+ {0.99, 24.725},
+ {1.00, 1e+12},
+ }},
+ // 12 dof
+ {12, {
+ {0.10, 6.3038},
+ {0.15, 7.11384},
+ {0.20, 7.80733},
+ {0.25, 8.43842},
+ {0.30, 9.03428},
+ {0.35, 9.61152},
+ {0.40, 10.182},
+ {0.45, 10.7553},
+ {0.50, 11.3403},
+ {0.55, 11.9463},
+ {0.60, 12.5838},
+ {0.65, 13.2661},
+ {0.70, 14.0111},
+ {0.75, 14.8454},
+ {0.80, 15.812},
+ {0.85, 16.9893},
+ {0.90, 18.5493},
+ {0.95, 21.0261},
+ {0.99, 26.217},
+ {1.00, 1e+12},
+ }},
+ // 13 dof
+ {13, {
+ {0.10, 7.0415},
+ {0.15, 7.90084},
+ {0.20, 8.63386},
+ {0.25, 9.29907},
+ {0.30, 9.92568},
+ {0.35, 10.5315},
+ {0.40, 11.1291},
+ {0.45, 11.7288},
+ {0.50, 12.3398},
+ {0.55, 12.9717},
+ {0.60, 13.6356},
+ {0.65, 14.3451},
+ {0.70, 15.1187},
+ {0.75, 15.9839},
+ {0.80, 16.9848},
+ {0.85, 18.202},
+ {0.90, 19.8119},
+ {0.95, 22.362},
+ {0.99, 27.6882},
+ {1.00, 1e+12},
+ }},
+ // 14 dof
+ {14, {
+ {0.10, 7.78953},
+ {0.15, 8.6963},
+ {0.20, 9.46733},
+ {0.25, 10.1653},
+ {0.30, 10.8215},
+ {0.35, 11.4548},
+ {0.40, 12.0785},
+ {0.45, 12.7034},
+ {0.50, 13.3393},
+ {0.55, 13.9961},
+ {0.60, 14.6853},
+ {0.65, 15.4209},
+ {0.70, 16.2221},
+ {0.75, 17.1169},
+ {0.80, 18.1508},
+ {0.85, 19.4062},
+ {0.90, 21.0641},
+ {0.95, 23.6848},
+ {0.99, 29.1412},
+ {1.00, 1e+12},
+ }},
+ // 15 dof
+ {15, {
+ {0.10, 8.54676},
+ {0.15, 9.49928},
+ {0.20, 10.307},
+ {0.25, 11.0365},
+ {0.30, 11.7212},
+ {0.35, 12.3809},
+ {0.40, 13.0297},
+ {0.45, 13.679},
+ {0.50, 14.3389},
+ {0.55, 15.0197},
+ {0.60, 15.7332},
+ {0.65, 16.494},
+ {0.70, 17.3217},
+ {0.75, 18.2451},
+ {0.80, 19.3107},
+ {0.85, 20.603},
+ {0.90, 22.3071},
+ {0.95, 24.9958},
+ {0.99, 30.5779},
+ {1.00, 1e+12},
+ }},
+ // 16 dof
+ {16, {
+ {0.10, 9.31224},
+ {0.15, 10.309},
+ {0.20, 11.1521},
+ {0.25, 11.9122},
+ {0.30, 12.6243},
+ {0.35, 13.3096},
+ {0.40, 13.9827},
+ {0.45, 14.6555},
+ {0.50, 15.3385},
+ {0.55, 16.0425},
+ {0.60, 16.7795},
+ {0.65, 17.5646},
+ {0.70, 18.4179},
+ {0.75, 19.3689},
+ {0.80, 20.4651},
+ {0.85, 21.7931},
+ {0.90, 23.5418},
+ {0.95, 26.2962},
+ {0.99, 31.9999},
+ {1.00, 1e+12},
+ }},
+ // 17 dof
+ {17, {
+ {0.10, 10.0852},
+ {0.15, 11.1249},
+ {0.20, 12.0023},
+ {0.25, 12.7919},
+ {0.30, 13.5307},
+ {0.35, 14.2407},
+ {0.40, 14.9373},
+ {0.45, 15.6328},
+ {0.50, 16.3382},
+ {0.55, 17.0646},
+ {0.60, 17.8244},
+ {0.65, 18.633},
+ {0.70, 19.511},
+ {0.75, 20.4887},
+ {0.80, 21.6146},
+ {0.85, 22.977},
+ {0.90, 24.769},
+ {0.95, 27.5871},
+ {0.99, 33.4087},
+ {1.00, 1e+12},
+ }},
+ // 18 dof
+ {18, {
+ {0.10, 10.8649},
+ {0.15, 11.9463},
+ {0.20, 12.857},
+ {0.25, 13.6753},
+ {0.30, 14.4399},
+ {0.35, 15.1738},
+ {0.40, 15.8932},
+ {0.45, 16.6108},
+ {0.50, 17.3379},
+ {0.55, 18.086},
+ {0.60, 18.8679},
+ {0.65, 19.6993},
+ {0.70, 20.6014},
+ {0.75, 21.6049},
+ {0.80, 22.7595},
+ {0.85, 24.1555},
+ {0.90, 25.9894},
+ {0.95, 28.8693},
+ {0.99, 34.8053},
+ {1.00, 1e+12},
+ }},
+ // 19 dof
+ {19, {
+ {0.10, 11.6509},
+ {0.15, 12.7727},
+ {0.20, 13.7158},
+ {0.25, 14.562},
+ {0.30, 15.3517},
+ {0.35, 16.1089},
+ {0.40, 16.8504},
+ {0.45, 17.5894},
+ {0.50, 18.3377},
+ {0.55, 19.1069},
+ {0.60, 19.9102},
+ {0.65, 20.7638},
+ {0.70, 21.6891},
+ {0.75, 22.7178},
+ {0.80, 23.9004},
+ {0.85, 25.3289},
+ {0.90, 27.2036},
+ {0.95, 30.1435},
+ {0.99, 36.1909},
+ {1.00, 1e+12},
+ }},
+ // 20 dof
+ {20, {
+ {0.10, 12.4426},
+ {0.15, 13.6039},
+ {0.20, 14.5784},
+ {0.25, 15.4518},
+ {0.30, 16.2659},
+ {0.35, 17.0458},
+ {0.40, 17.8088},
+ {0.45, 18.5687},
+ {0.50, 19.3374},
+ {0.55, 20.1272},
+ {0.60, 20.9514},
+ {0.65, 21.8265},
+ {0.70, 22.7745},
+ {0.75, 23.8277},
+ {0.80, 25.0375},
+ {0.85, 26.4976},
+ {0.90, 28.412},
+ {0.95, 31.4104},
+ {0.99, 37.5662},
+ {1.00, 1e+12},
+ }},
+ // 21 dof
+ {21, {
+ {0.10, 13.2396},
+ {0.15, 14.4393},
+ {0.20, 15.4446},
+ {0.25, 16.3444},
+ {0.30, 17.1823},
+ {0.35, 17.9843},
+ {0.40, 18.7683},
+ {0.45, 19.5485},
+ {0.50, 20.3372},
+ {0.55, 21.147},
+ {0.60, 21.9915},
+ {0.65, 22.8876},
+ {0.70, 23.8578},
+ {0.75, 24.9348},
+ {0.80, 26.1711},
+ {0.85, 27.662},
+ {0.90, 29.6151},
+ {0.95, 32.6706},
+ {0.99, 38.9322},
+ {1.00, 1e+12},
+ }},
+ // 22 dof
+ {22, {
+ {0.10, 14.0415},
+ {0.15, 15.2788},
+ {0.20, 16.314},
+ {0.25, 17.2396},
+ {0.30, 18.1007},
+ {0.35, 18.9243},
+ {0.40, 19.7288},
+ {0.45, 20.5288},
+ {0.50, 21.337},
+ {0.55, 22.1663},
+ {0.60, 23.0307},
+ {0.65, 23.9473},
+ {0.70, 24.939},
+ {0.75, 26.0393},
+ {0.80, 27.3015},
+ {0.85, 28.8225},
+ {0.90, 30.8133},
+ {0.95, 33.9244},
+ {0.99, 40.2894},
+ {1.00, 1e+12},
+ }},
+ // 23 dof
+ {23, {
+ {0.10, 14.848},
+ {0.15, 16.1219},
+ {0.20, 17.1865},
+ {0.25, 18.1373},
+ {0.30, 19.0211},
+ {0.35, 19.8657},
+ {0.40, 20.6902},
+ {0.45, 21.5096},
+ {0.50, 22.3369},
+ {0.55, 23.1852},
+ {0.60, 24.0689},
+ {0.65, 25.0055},
+ {0.70, 26.0184},
+ {0.75, 27.1413},
+ {0.80, 28.4288},
+ {0.85, 29.9792},
+ {0.90, 32.0069},
+ {0.95, 35.1725},
+ {0.99, 41.6384},
+ {1.00, 1e+12},
+ }},
+ // 24 dof
+ {24, {
+ {0.10, 15.6587},
+ {0.15, 16.9686},
+ {0.20, 18.0618},
+ {0.25, 19.0373},
+ {0.30, 19.9432},
+ {0.35, 20.8084},
+ {0.40, 21.6525},
+ {0.45, 22.4908},
+ {0.50, 23.3367},
+ {0.55, 24.2037},
+ {0.60, 25.1063},
+ {0.65, 26.0625},
+ {0.70, 27.096},
+ {0.75, 28.2412},
+ {0.80, 29.5533},
+ {0.85, 31.1325},
+ {0.90, 33.1962},
+ {0.95, 36.415},
+ {0.99, 42.9798},
+ {1.00, 1e+12},
+ }},
+ // 25 dof
+ {25, {
+ {0.10, 16.4734},
+ {0.15, 17.8184},
+ {0.20, 18.9398},
+ {0.25, 19.9393},
+ {0.30, 20.867},
+ {0.35, 21.7524},
+ {0.40, 22.6156},
+ {0.45, 23.4724},
+ {0.50, 24.3366},
+ {0.55, 25.2218},
+ {0.60, 26.143},
+ {0.65, 27.1183},
+ {0.70, 28.1719},
+ {0.75, 29.3389},
+ {0.80, 30.6752},
+ {0.85, 32.2825},
+ {0.90, 34.3816},
+ {0.95, 37.6525},
+ {0.99, 44.3141},
+ {1.00, 1e+12},
+ }},
+ // 26 dof
+ {26, {
+ {0.10, 17.2919},
+ {0.15, 18.6714},
+ {0.20, 19.8202},
+ {0.25, 20.8434},
+ {0.30, 21.7924},
+ {0.35, 22.6975},
+ {0.40, 23.5794},
+ {0.45, 24.4544},
+ {0.50, 25.3365},
+ {0.55, 26.2395},
+ {0.60, 27.1789},
+ {0.65, 28.173},
+ {0.70, 29.2463},
+ {0.75, 30.4346},
+ {0.80, 31.7946},
+ {0.85, 33.4295},
+ {0.90, 35.5632},
+ {0.95, 38.8851},
+ {0.99, 45.6417},
+ {1.00, 1e+12},
+ }},
+ // 27 dof
+ {27, {
+ {0.10, 18.1139},
+ {0.15, 19.5272},
+ {0.20, 20.703},
+ {0.25, 21.7494},
+ {0.30, 22.7192},
+ {0.35, 23.6437},
+ {0.40, 24.544},
+ {0.45, 25.4368},
+ {0.50, 26.3363},
+ {0.55, 27.2569},
+ {0.60, 28.2141},
+ {0.65, 29.2266},
+ {0.70, 30.3193},
+ {0.75, 31.5284},
+ {0.80, 32.9117},
+ {0.85, 34.5736},
+ {0.90, 36.7412},
+ {0.95, 40.1133},
+ {0.99, 46.9629},
+ {1.00, 1e+12},
+ }},
+ // 28 dof
+ {28, {
+ {0.10, 18.9392},
+ {0.15, 20.3857},
+ {0.20, 21.588},
+ {0.25, 22.6572},
+ {0.30, 23.6475},
+ {0.35, 24.5909},
+ {0.40, 25.5093},
+ {0.45, 26.4195},
+ {0.50, 27.3362},
+ {0.55, 28.274},
+ {0.60, 29.2486},
+ {0.65, 30.2791},
+ {0.70, 31.3909},
+ {0.75, 32.6205},
+ {0.80, 34.0266},
+ {0.85, 35.715},
+ {0.90, 37.9159},
+ {0.95, 41.3371},
+ {0.99, 48.2782},
+ {1.00, 1e+12},
+ }},
+ // 29 dof
+ {29, {
+ {0.10, 19.7677},
+ {0.15, 21.2468},
+ {0.20, 22.4751},
+ {0.25, 23.5666},
+ {0.30, 24.577},
+ {0.35, 25.5391},
+ {0.40, 26.4751},
+ {0.45, 27.4025},
+ {0.50, 28.3361},
+ {0.55, 29.2908},
+ {0.60, 30.2825},
+ {0.65, 31.3308},
+ {0.70, 32.4612},
+ {0.75, 33.7109},
+ {0.80, 35.1394},
+ {0.85, 36.8538},
+ {0.90, 39.0875},
+ {0.95, 42.557},
+ {0.99, 49.5879},
+ {1.00, 1e+12},
+ }},
+ // 30 dof
+ {30, {
+ {0.10, 20.5992},
+ {0.15, 22.1103},
+ {0.20, 23.3641},
+ {0.25, 24.4776},
+ {0.30, 25.5078},
+ {0.35, 26.4881},
+ {0.40, 27.4416},
+ {0.45, 28.3858},
+ {0.50, 29.336},
+ {0.55, 30.3073},
+ {0.60, 31.3159},
+ {0.65, 32.3815},
+ {0.70, 33.5302},
+ {0.75, 34.7997},
+ {0.80, 36.2502},
+ {0.85, 37.9903},
+ {0.90, 40.256},
+ {0.95, 43.773},
+ {0.99, 50.8922},
+ {1.00, 1e+12},
+ }},
+};
+
+static std::map<int, std::map<double, double>> chisq_2_CI = {
+ // 1 dof
+ {1, {
+ {0.0157908, 0.10},
+ {0.0357658, 0.15},
+ {0.0641848, 0.20},
+ {0.101531, 0.25},
+ {0.148472, 0.30},
+ {0.2059, 0.35},
+ {0.274996, 0.40},
+ {0.357317, 0.45},
+ {0.454936, 0.50},
+ {0.570652, 0.55},
+ {0.708326, 0.60},
+ {0.873457, 0.65},
+ {1.07419, 0.70},
+ {1.3233, 0.75},
+ {1.64237, 0.80},
+ {2.07225, 0.85},
+ {2.70554, 0.90},
+ {3.84146, 0.95},
+ {6.6349, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 2 dof
+ {2, {
+ {0.210721, 0.10},
+ {0.325038, 0.15},
+ {0.446287, 0.20},
+ {0.575364, 0.25},
+ {0.71335, 0.30},
+ {0.861566, 0.35},
+ {1.02165, 0.40},
+ {1.19567, 0.45},
+ {1.38629, 0.50},
+ {1.59702, 0.55},
+ {1.83258, 0.60},
+ {2.09964, 0.65},
+ {2.40795, 0.70},
+ {2.77259, 0.75},
+ {3.21888, 0.80},
+ {3.79424, 0.85},
+ {4.60517, 0.90},
+ {5.99146, 0.95},
+ {9.21034, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 3 dof
+ {3, {
+ {0.584374, 0.10},
+ {0.797771, 0.15},
+ {1.00517, 0.20},
+ {1.21253, 0.25},
+ {1.42365, 0.30},
+ {1.64158, 0.35},
+ {1.86917, 0.40},
+ {2.10947, 0.45},
+ {2.36597, 0.50},
+ {2.64301, 0.55},
+ {2.94617, 0.60},
+ {3.28311, 0.65},
+ {3.66487, 0.70},
+ {4.10834, 0.75},
+ {4.64163, 0.80},
+ {5.31705, 0.85},
+ {6.25139, 0.90},
+ {7.81473, 0.95},
+ {11.3449, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 4 dof
+ {4, {
+ {1.06362, 0.10},
+ {1.36648, 0.15},
+ {1.64878, 0.20},
+ {1.92256, 0.25},
+ {2.1947, 0.30},
+ {2.47009, 0.35},
+ {2.75284, 0.40},
+ {3.04695, 0.45},
+ {3.35669, 0.50},
+ {3.68713, 0.55},
+ {4.04463, 0.60},
+ {4.43769, 0.65},
+ {4.87843, 0.70},
+ {5.38527, 0.75},
+ {5.98862, 0.80},
+ {6.74488, 0.85},
+ {7.77944, 0.90},
+ {9.48773, 0.95},
+ {13.2767, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 5 dof
+ {5, {
+ {1.61031, 0.10},
+ {1.99382, 0.15},
+ {2.34253, 0.20},
+ {2.6746, 0.25},
+ {2.99991, 0.30},
+ {3.32511, 0.35},
+ {3.6555, 0.40},
+ {3.99594, 0.45},
+ {4.35146, 0.50},
+ {4.72776, 0.55},
+ {5.13187, 0.60},
+ {5.57307, 0.65},
+ {6.06443, 0.70},
+ {6.62568, 0.75},
+ {7.28928, 0.80},
+ {8.1152, 0.85},
+ {9.23636, 0.90},
+ {11.0705, 0.95},
+ {15.0863, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 6 dof
+ {6, {
+ {2.20413, 0.10},
+ {2.66127, 0.15},
+ {3.07009, 0.20},
+ {3.4546, 0.25},
+ {3.82755, 0.30},
+ {4.19727, 0.35},
+ {4.57015, 0.40},
+ {4.95188, 0.45},
+ {5.34812, 0.50},
+ {5.7652, 0.55},
+ {6.21076, 0.60},
+ {6.69476, 0.65},
+ {7.23114, 0.70},
+ {7.8408, 0.75},
+ {8.55806, 0.80},
+ {9.4461, 0.85},
+ {10.6446, 0.90},
+ {12.5916, 0.95},
+ {16.8119, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 7 dof
+ {7, {
+ {2.83311, 0.10},
+ {3.35828, 0.15},
+ {3.82232, 0.20},
+ {4.25485, 0.25},
+ {4.67133, 0.30},
+ {5.08165, 0.35},
+ {5.49323, 0.40},
+ {5.91252, 0.45},
+ {6.34581, 0.50},
+ {6.79997, 0.55},
+ {7.28321, 0.60},
+ {7.80612, 0.65},
+ {8.38343, 0.70},
+ {9.03715, 0.75},
+ {9.80325, 0.80},
+ {10.7479, 0.85},
+ {12.017, 0.90},
+ {14.0671, 0.95},
+ {18.4753, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 8 dof
+ {8, {
+ {3.48954, 0.10},
+ {4.0782, 0.15},
+ {4.59357, 0.20},
+ {5.07064, 0.25},
+ {5.52742, 0.30},
+ {5.97529, 0.35},
+ {6.42265, 0.40},
+ {6.87663, 0.45},
+ {7.34412, 0.50},
+ {7.83251, 0.55},
+ {8.35053, 0.60},
+ {8.90936, 0.65},
+ {9.52446, 0.70},
+ {10.2189, 0.75},
+ {11.0301, 0.80},
+ {12.0271, 0.85},
+ {13.3616, 0.90},
+ {15.5073, 0.95},
+ {20.0902, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 9 dof
+ {9, {
+ {4.16816, 0.10},
+ {4.81652, 0.15},
+ {5.38005, 0.20},
+ {5.89883, 0.25},
+ {6.39331, 0.30},
+ {6.87627, 0.35},
+ {7.35703, 0.40},
+ {7.84342, 0.45},
+ {8.34283, 0.50},
+ {8.86317, 0.55},
+ {9.41364, 0.60},
+ {10.006, 0.65},
+ {10.6564, 0.70},
+ {11.3888, 0.75},
+ {12.2421, 0.80},
+ {13.288, 0.85},
+ {14.6837, 0.90},
+ {16.919, 0.95},
+ {21.666, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 10 dof
+ {10, {
+ {4.86518, 0.10},
+ {5.57006, 0.15},
+ {6.17908, 0.20},
+ {6.7372, 0.25},
+ {7.26722, 0.30},
+ {7.78324, 0.35},
+ {8.29547, 0.40},
+ {8.81235, 0.45},
+ {9.34182, 0.50},
+ {9.89222, 0.55},
+ {10.4732, 0.60},
+ {11.0971, 0.65},
+ {11.7807, 0.70},
+ {12.5489, 0.75},
+ {13.442, 0.80},
+ {14.5339, 0.85},
+ {15.9872, 0.90},
+ {18.307, 0.95},
+ {23.2093, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 11 dof
+ {11, {
+ {5.57778, 0.10},
+ {6.33643, 0.15},
+ {6.98867, 0.20},
+ {7.58414, 0.25},
+ {8.14787, 0.30},
+ {8.69524, 0.35},
+ {9.23729, 0.40},
+ {9.78306, 0.45},
+ {10.341, 0.50},
+ {10.9199, 0.55},
+ {11.5298, 0.60},
+ {12.1836, 0.65},
+ {12.8987, 0.70},
+ {13.7007, 0.75},
+ {14.6314, 0.80},
+ {15.7671, 0.85},
+ {17.275, 0.90},
+ {19.6751, 0.95},
+ {24.725, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 12 dof
+ {12, {
+ {6.3038, 0.10},
+ {7.11384, 0.15},
+ {7.80733, 0.20},
+ {8.43842, 0.25},
+ {9.03428, 0.30},
+ {9.61152, 0.35},
+ {10.182, 0.40},
+ {10.7553, 0.45},
+ {11.3403, 0.50},
+ {11.9463, 0.55},
+ {12.5838, 0.60},
+ {13.2661, 0.65},
+ {14.0111, 0.70},
+ {14.8454, 0.75},
+ {15.812, 0.80},
+ {16.9893, 0.85},
+ {18.5493, 0.90},
+ {21.0261, 0.95},
+ {26.217, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 13 dof
+ {13, {
+ {7.0415, 0.10},
+ {7.90084, 0.15},
+ {8.63386, 0.20},
+ {9.29907, 0.25},
+ {9.92568, 0.30},
+ {10.5315, 0.35},
+ {11.1291, 0.40},
+ {11.7288, 0.45},
+ {12.3398, 0.50},
+ {12.9717, 0.55},
+ {13.6356, 0.60},
+ {14.3451, 0.65},
+ {15.1187, 0.70},
+ {15.9839, 0.75},
+ {16.9848, 0.80},
+ {18.202, 0.85},
+ {19.8119, 0.90},
+ {22.362, 0.95},
+ {27.6882, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 14 dof
+ {14, {
+ {7.78953, 0.10},
+ {8.6963, 0.15},
+ {9.46733, 0.20},
+ {10.1653, 0.25},
+ {10.8215, 0.30},
+ {11.4548, 0.35},
+ {12.0785, 0.40},
+ {12.7034, 0.45},
+ {13.3393, 0.50},
+ {13.9961, 0.55},
+ {14.6853, 0.60},
+ {15.4209, 0.65},
+ {16.2221, 0.70},
+ {17.1169, 0.75},
+ {18.1508, 0.80},
+ {19.4062, 0.85},
+ {21.0641, 0.90},
+ {23.6848, 0.95},
+ {29.1412, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 15 dof
+ {15, {
+ {8.54676, 0.10},
+ {9.49928, 0.15},
+ {10.307, 0.20},
+ {11.0365, 0.25},
+ {11.7212, 0.30},
+ {12.3809, 0.35},
+ {13.0297, 0.40},
+ {13.679, 0.45},
+ {14.3389, 0.50},
+ {15.0197, 0.55},
+ {15.7332, 0.60},
+ {16.494, 0.65},
+ {17.3217, 0.70},
+ {18.2451, 0.75},
+ {19.3107, 0.80},
+ {20.603, 0.85},
+ {22.3071, 0.90},
+ {24.9958, 0.95},
+ {30.5779, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 16 dof
+ {16, {
+ {9.31224, 0.10},
+ {10.309, 0.15},
+ {11.1521, 0.20},
+ {11.9122, 0.25},
+ {12.6243, 0.30},
+ {13.3096, 0.35},
+ {13.9827, 0.40},
+ {14.6555, 0.45},
+ {15.3385, 0.50},
+ {16.0425, 0.55},
+ {16.7795, 0.60},
+ {17.5646, 0.65},
+ {18.4179, 0.70},
+ {19.3689, 0.75},
+ {20.4651, 0.80},
+ {21.7931, 0.85},
+ {23.5418, 0.90},
+ {26.2962, 0.95},
+ {31.9999, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 17 dof
+ {17, {
+ {10.0852, 0.10},
+ {11.1249, 0.15},
+ {12.0023, 0.20},
+ {12.7919, 0.25},
+ {13.5307, 0.30},
+ {14.2407, 0.35},
+ {14.9373, 0.40},
+ {15.6328, 0.45},
+ {16.3382, 0.50},
+ {17.0646, 0.55},
+ {17.8244, 0.60},
+ {18.633, 0.65},
+ {19.511, 0.70},
+ {20.4887, 0.75},
+ {21.6146, 0.80},
+ {22.977, 0.85},
+ {24.769, 0.90},
+ {27.5871, 0.95},
+ {33.4087, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 18 dof
+ {18, {
+ {10.8649, 0.10},
+ {11.9463, 0.15},
+ {12.857, 0.20},
+ {13.6753, 0.25},
+ {14.4399, 0.30},
+ {15.1738, 0.35},
+ {15.8932, 0.40},
+ {16.6108, 0.45},
+ {17.3379, 0.50},
+ {18.086, 0.55},
+ {18.8679, 0.60},
+ {19.6993, 0.65},
+ {20.6014, 0.70},
+ {21.6049, 0.75},
+ {22.7595, 0.80},
+ {24.1555, 0.85},
+ {25.9894, 0.90},
+ {28.8693, 0.95},
+ {34.8053, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 19 dof
+ {19, {
+ {11.6509, 0.10},
+ {12.7727, 0.15},
+ {13.7158, 0.20},
+ {14.562, 0.25},
+ {15.3517, 0.30},
+ {16.1089, 0.35},
+ {16.8504, 0.40},
+ {17.5894, 0.45},
+ {18.3377, 0.50},
+ {19.1069, 0.55},
+ {19.9102, 0.60},
+ {20.7638, 0.65},
+ {21.6891, 0.70},
+ {22.7178, 0.75},
+ {23.9004, 0.80},
+ {25.3289, 0.85},
+ {27.2036, 0.90},
+ {30.1435, 0.95},
+ {36.1909, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 20 dof
+ {20, {
+ {12.4426, 0.10},
+ {13.6039, 0.15},
+ {14.5784, 0.20},
+ {15.4518, 0.25},
+ {16.2659, 0.30},
+ {17.0458, 0.35},
+ {17.8088, 0.40},
+ {18.5687, 0.45},
+ {19.3374, 0.50},
+ {20.1272, 0.55},
+ {20.9514, 0.60},
+ {21.8265, 0.65},
+ {22.7745, 0.70},
+ {23.8277, 0.75},
+ {25.0375, 0.80},
+ {26.4976, 0.85},
+ {28.412, 0.90},
+ {31.4104, 0.95},
+ {37.5662, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 21 dof
+ {21, {
+ {13.2396, 0.10},
+ {14.4393, 0.15},
+ {15.4446, 0.20},
+ {16.3444, 0.25},
+ {17.1823, 0.30},
+ {17.9843, 0.35},
+ {18.7683, 0.40},
+ {19.5485, 0.45},
+ {20.3372, 0.50},
+ {21.147, 0.55},
+ {21.9915, 0.60},
+ {22.8876, 0.65},
+ {23.8578, 0.70},
+ {24.9348, 0.75},
+ {26.1711, 0.80},
+ {27.662, 0.85},
+ {29.6151, 0.90},
+ {32.6706, 0.95},
+ {38.9322, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 22 dof
+ {22, {
+ {14.0415, 0.10},
+ {15.2788, 0.15},
+ {16.314, 0.20},
+ {17.2396, 0.25},
+ {18.1007, 0.30},
+ {18.9243, 0.35},
+ {19.7288, 0.40},
+ {20.5288, 0.45},
+ {21.337, 0.50},
+ {22.1663, 0.55},
+ {23.0307, 0.60},
+ {23.9473, 0.65},
+ {24.939, 0.70},
+ {26.0393, 0.75},
+ {27.3015, 0.80},
+ {28.8225, 0.85},
+ {30.8133, 0.90},
+ {33.9244, 0.95},
+ {40.2894, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 23 dof
+ {23, {
+ {14.848, 0.10},
+ {16.1219, 0.15},
+ {17.1865, 0.20},
+ {18.1373, 0.25},
+ {19.0211, 0.30},
+ {19.8657, 0.35},
+ {20.6902, 0.40},
+ {21.5096, 0.45},
+ {22.3369, 0.50},
+ {23.1852, 0.55},
+ {24.0689, 0.60},
+ {25.0055, 0.65},
+ {26.0184, 0.70},
+ {27.1413, 0.75},
+ {28.4288, 0.80},
+ {29.9792, 0.85},
+ {32.0069, 0.90},
+ {35.1725, 0.95},
+ {41.6384, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 24 dof
+ {24, {
+ {15.6587, 0.10},
+ {16.9686, 0.15},
+ {18.0618, 0.20},
+ {19.0373, 0.25},
+ {19.9432, 0.30},
+ {20.8084, 0.35},
+ {21.6525, 0.40},
+ {22.4908, 0.45},
+ {23.3367, 0.50},
+ {24.2037, 0.55},
+ {25.1063, 0.60},
+ {26.0625, 0.65},
+ {27.096, 0.70},
+ {28.2412, 0.75},
+ {29.5533, 0.80},
+ {31.1325, 0.85},
+ {33.1962, 0.90},
+ {36.415, 0.95},
+ {42.9798, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 25 dof
+ {25, {
+ {16.4734, 0.10},
+ {17.8184, 0.15},
+ {18.9398, 0.20},
+ {19.9393, 0.25},
+ {20.867, 0.30},
+ {21.7524, 0.35},
+ {22.6156, 0.40},
+ {23.4724, 0.45},
+ {24.3366, 0.50},
+ {25.2218, 0.55},
+ {26.143, 0.60},
+ {27.1183, 0.65},
+ {28.1719, 0.70},
+ {29.3389, 0.75},
+ {30.6752, 0.80},
+ {32.2825, 0.85},
+ {34.3816, 0.90},
+ {37.6525, 0.95},
+ {44.3141, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 26 dof
+ {26, {
+ {17.2919, 0.10},
+ {18.6714, 0.15},
+ {19.8202, 0.20},
+ {20.8434, 0.25},
+ {21.7924, 0.30},
+ {22.6975, 0.35},
+ {23.5794, 0.40},
+ {24.4544, 0.45},
+ {25.3365, 0.50},
+ {26.2395, 0.55},
+ {27.1789, 0.60},
+ {28.173, 0.65},
+ {29.2463, 0.70},
+ {30.4346, 0.75},
+ {31.7946, 0.80},
+ {33.4295, 0.85},
+ {35.5632, 0.90},
+ {38.8851, 0.95},
+ {45.6417, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 27 dof
+ {27, {
+ {18.1139, 0.10},
+ {19.5272, 0.15},
+ {20.703, 0.20},
+ {21.7494, 0.25},
+ {22.7192, 0.30},
+ {23.6437, 0.35},
+ {24.544, 0.40},
+ {25.4368, 0.45},
+ {26.3363, 0.50},
+ {27.2569, 0.55},
+ {28.2141, 0.60},
+ {29.2266, 0.65},
+ {30.3193, 0.70},
+ {31.5284, 0.75},
+ {32.9117, 0.80},
+ {34.5736, 0.85},
+ {36.7412, 0.90},
+ {40.1133, 0.95},
+ {46.9629, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 28 dof
+ {28, {
+ {18.9392, 0.10},
+ {20.3857, 0.15},
+ {21.588, 0.20},
+ {22.6572, 0.25},
+ {23.6475, 0.30},
+ {24.5909, 0.35},
+ {25.5093, 0.40},
+ {26.4195, 0.45},
+ {27.3362, 0.50},
+ {28.274, 0.55},
+ {29.2486, 0.60},
+ {30.2791, 0.65},
+ {31.3909, 0.70},
+ {32.6205, 0.75},
+ {34.0266, 0.80},
+ {35.715, 0.85},
+ {37.9159, 0.90},
+ {41.3371, 0.95},
+ {48.2782, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 29 dof
+ {29, {
+ {19.7677, 0.10},
+ {21.2468, 0.15},
+ {22.4751, 0.20},
+ {23.5666, 0.25},
+ {24.577, 0.30},
+ {25.5391, 0.35},
+ {26.4751, 0.40},
+ {27.4025, 0.45},
+ {28.3361, 0.50},
+ {29.2908, 0.55},
+ {30.2825, 0.60},
+ {31.3308, 0.65},
+ {32.4612, 0.70},
+ {33.7109, 0.75},
+ {35.1394, 0.80},
+ {36.8538, 0.85},
+ {39.0875, 0.90},
+ {42.557, 0.95},
+ {49.5879, 0.99},
+ { 1e+12, 1.00},
+ }},
+ // 30 dof
+ {30, {
+ {20.5992, 0.10},
+ {22.1103, 0.15},
+ {23.3641, 0.20},
+ {24.4776, 0.25},
+ {25.5078, 0.30},
+ {26.4881, 0.35},
+ {27.4416, 0.40},
+ {28.3858, 0.45},
+ {29.336, 0.50},
+ {30.3073, 0.55},
+ {31.3159, 0.60},
+ {32.3815, 0.65},
+ {33.5302, 0.70},
+ {34.7997, 0.75},
+ {36.2502, 0.80},
+ {37.9903, 0.85},
+ {40.256, 0.90},
+ {43.773, 0.95},
+ {50.8922, 0.99},
+ { 1e+12, 1.00},
+ }},
+};
+
+}// namespace GnssUtils
+
+#endif /* INCLUDE_GNSS_UTILS_UTILS_CHISQUARE_CI_MAPS_H_ */
diff --git a/include/gnss_utils/utils/rcv_position.h b/include/gnss_utils/utils/rcv_position.h
new file mode 100644
index 0000000000000000000000000000000000000000..bf84a9911387d3b5c834212811f39dd63a3331c5
--- /dev/null
+++ b/include/gnss_utils/utils/rcv_position.h
@@ -0,0 +1,69 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * transfromation.h
+ *
+ * Created on: April 3, 2020
+ * Author: Joan Vallvé, Pep MartÃ-Saumell
+ */
+
+#ifndef INCLUDE_GNSS_UTILS_UTILS_RCV_POSITION_H_
+#define INCLUDE_GNSS_UTILS_UTILS_RCV_POSITION_H_
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <Eigen/Sparse>
+
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+#include "gnss_utils/utils/transformations.h"
+#include "gnss_utils/gnss_utils.h"
+
+namespace GnssUtils
+{
+
+ComputePosOutput computePos(const Observations& _observations, const Navigation& _navigation, const Options& _opt);
+ComputePosOutput computePos(const Observations& _observations, const Navigation& _navigation, const prcopt_t& _prcopt);
+
+// ComputePosOutput computePosOwn(const Observations & _observations,
+// Navigation & _navigation,
+// const prcopt_t & _prcopt);
+
+// int pntposOwn(const obsd_t *obs, int n, const nav_t *nav,
+// const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
+// char *msg);
+
+int estposOwn(const obsd_t* obs,
+ int n,
+ const double* rs,
+ const double* dts,
+ const double* vare,
+ const int* svh,
+ const nav_t* nav,
+ const prcopt_t* opt,
+ sol_t* sol,
+ double* azel,
+ int* vsat,
+ double* resp,
+ char* msg);
+} // namespace GnssUtils
+#endif // INCLUDE_GNSS_UTILS_UTILS_RCV_POSITION_H_
diff --git a/include/gnss_utils/utils/satellite.h b/include/gnss_utils/utils/satellite.h
new file mode 100644
index 0000000000000000000000000000000000000000..0162a11eebf598e71f70d082fe15c53cc0298ce5
--- /dev/null
+++ b/include/gnss_utils/utils/satellite.h
@@ -0,0 +1,66 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * sat_position.h
+ *
+ * Created on: April 3, 2020
+ * Author: Joan Vallvé, Pep MartÃ-Saumell
+ */
+
+#ifndef INCLUDE_GNSS_UTILS_UTILS_SAT_POSITION_H_
+#define INCLUDE_GNSS_UTILS_UTILS_SAT_POSITION_H_
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+#include "gnss_utils/gnss_utils.h"
+
+namespace GnssUtils
+{
+ // forward declarations
+ class Observations;
+ class Navigation;
+
+ Satellites computeSatellites(const Observations& obs,
+ const Navigation& nav,
+ const int& eph_opt); // see rtklib.h L396
+
+ Eigen::Vector2d computeAzel(const Eigen::Vector3d& sat_ecef, const Eigen::Vector3d& receiver_ecef);
+
+ Eigen::Vector2d computeAzel(const Satellite& sat, const Eigen::Vector3d& receiver_ecef);
+
+ Azels computeAzels(const Satellites& sats, const Eigen::Vector3d& receiver_ecef);
+
+ struct Satellite
+ {
+ int sys;
+ int sat;
+ Eigen::Vector3d pos;
+ Eigen::Vector3d vel;
+ double var;
+ double clock_bias;
+ double clock_drift;
+ int svh;
+ };
+
+} // namespace GnssUtils
+#endif // INCLUDE_GNSS_UTILS_UTILS_SAT_POSITION_H_
diff --git a/include/gnss_utils/utils/transformations.h b/include/gnss_utils/utils/transformations.h
new file mode 100644
index 0000000000000000000000000000000000000000..92533d1ae2c82b38b852fa762cb48b6662f6eb5a
--- /dev/null
+++ b/include/gnss_utils/utils/transformations.h
@@ -0,0 +1,52 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * transfromation.h
+ *
+ * Created on: April 3, 2020
+ * Author: Joan Vallvé, Pep MartÃ-Saumell
+ */
+
+#ifndef INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
+#define INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <Eigen/Sparse>
+#include "gnss_utils/gnss_utils.h"
+
+namespace GnssUtils
+{
+Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef);
+Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon);
+Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d& _cov_ecef);
+Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d& _cov_enu);
+
+void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_ECEF);
+void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_ECEF);
+
+} // namespace GnssUtils
+#endif // INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
diff --git a/include/gnss_utils/utils/utils.h b/include/gnss_utils/utils/utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..7c29dd40319343a388a6b62da45ae1e7b8a266cb
--- /dev/null
+++ b/include/gnss_utils/utils/utils.h
@@ -0,0 +1,238 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef INCLUDE_GNSS_UTILS_UTILS_UTILS_H_
+#define INCLUDE_GNSS_UTILS_UTILS_UTILS_H_
+
+#include <vector>
+#include <iostream>
+#include <memory>
+#include <string>
+
+#include "gnss_utils/internal/config.h"
+#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/observations.h"
+
+#define ARRAY_SIZE(arr) sizeof(arr) / sizeof(arr[0])
+#define ARRAY2D_SIZE(arr) sizeof(arr) / sizeof(arr[0][0])
+#define ARRAY3D_SIZE(arr) sizeof(arr) / sizeof(arr[0][0][0])
+#define ARRAY2D_NROWS(arr) sizeof(arr) / sizeof(arr[0])
+#define ARRAY2D_NCOLS(arr) sizeof(arr[0]) / sizeof(arr[0][0])
+#define GNSSUTILS_MSG "--GnssUtils--"
+
+namespace GnssUtils
+{
+void print(std::string& _msg);
+
+template <typename T, size_t size>
+void printArray(std::string name, T (&array)[size])
+{
+ std::cout << name << "[";
+ for (int ii = 0; ii < size; ++ii)
+ {
+ if (std::is_same<T, char>::value or std::is_same<T, unsigned char>::value)
+ std::cout << (int)array[ii];
+ else
+ std::cout << array[ii];
+ if (ii == size - 1)
+ std::cout << "] \n";
+ else
+ std::cout << ",";
+ }
+}
+
+template <typename T>
+inline bool addToArray(const T& new_element, T*& array, int& n, int& nmax)
+{
+ // std::cout << "addToArray: n = " << n << " nmax = " << nmax << "\n";
+ // "inspired" from RTKLIB rinex.c
+ T* array_ref;
+ if (nmax <= n)
+ {
+ // std::cout << "addToArray: nmax <= n\n";
+ nmax += 1024;
+ if (!(array_ref = (T*)realloc(array, sizeof(T) * nmax)))
+ {
+ printf("addToArray malloc error: n=%d\n", nmax);
+ free(array);
+ array = NULL;
+ n = nmax = 0;
+ return false;
+ }
+ // std::cout << "addToArray: assigning reallocated array\n";
+ array = array_ref;
+ }
+ // std::cout << "addToArray: adding element " << n << "\n";
+ array[n++] = new_element;
+ // std::cout << "addToArray: added!\n";
+ return true;
+}
+
+template <typename T>
+inline bool copyArray(const T* array_in, const int& n_in, T*& array_out, int& n_out, int& nmax_out)
+{
+ // std::cout << "copyArray: " << n_in << " elements\n";
+ if (array_in == NULL)
+ return false;
+
+ // std::cout << "copyArray: array in not null\n";
+
+ for (int i = 0; i < n_in; i++)
+ {
+ // std::cout << "copyArray: adding element " << i << "\n";
+ if (!addToArray<T>(array_in[i], array_out, n_out, nmax_out))
+ {
+ // std::cout << "copyArray: failed to add..\n";
+ return false;
+ }
+ // std::cout << "copyArray: n_out = " << n_out << " nmax_out = " << nmax_out << "\n";
+ }
+
+ // std::cout << "copyArray: all copied\n";
+
+ return true;
+}
+
+template <typename T>
+void freeArray(T*& array, int& n, int& nmax)
+{
+ if (array != NULL)
+ free(array);
+ array = NULL;
+ n = nmax = 0;
+}
+
+template <typename T>
+bool equalArray(const T* array_1, const T* array_2, const int& n1, const int& n2)
+{
+ if (n1 != n2)
+ return false;
+
+ for (int i = 0; i < n1; ++i)
+ {
+ if (array_1[i] != array_2[i])
+ return false;
+ }
+
+ return true;
+}
+
+template <typename T>
+bool equalArray2d(const T* array_1,
+ const T* array_2,
+ const int& nrow_1,
+ const int& ncol_1,
+ const int& nrow_2,
+ const int& ncol_2)
+{
+ if (nrow_1 != nrow_2)
+ return false;
+ if (ncol_1 != ncol_2)
+ return false;
+
+ for (int i = 0; i < nrow_1; ++i)
+ {
+ for (int j = 0; j < ncol_1; ++j)
+ {
+ if (*(array_1 + i * ncol_1 + j) != *(array_2 + i * ncol_1 + j))
+ return false;
+ }
+ }
+
+ return true;
+}
+
+template <typename T>
+bool equalArray3d(const T* array_1,
+ const T* array_2,
+ const int& nmat_1,
+ const int& nrow_1,
+ const int& ncol_1,
+ const int& nmat_2,
+ const int& nrow_2,
+ const int& ncol_2)
+{
+ if (nmat_1 != nmat_2)
+ return false;
+ if (nrow_1 != nrow_2)
+ return false;
+ if (ncol_1 != ncol_2)
+ return false;
+
+ for (int i = 0; i < nmat_1; ++i)
+ {
+ for (int j = 0; j < nrow_1; ++j)
+ {
+ for (int k = 0; k < ncol_1; ++k)
+ {
+ if (*(array_1 + i * ncol_1 * nrow_1 + j * ncol_1 + k) != *(array_2 + i * ncol_1 * nrow_1 + j * ncol_1 + k))
+ return false;
+ }
+ }
+ }
+
+ return true;
+}
+
+std::vector<Observations>
+loadObservationsFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt);
+
+} // namespace GnssUtils
+
+bool operator==(const gtime_t& time1, const gtime_t& time2);
+bool operator!=(const gtime_t& time1, const gtime_t& time2);
+
+bool operator==(const obsd_t& obs1, const obsd_t& obs2);
+bool operator!=(const obsd_t& obs1, const obsd_t& obs2);
+bool operator==(const obs_t& obs1, const obs_t& obs2);
+bool operator!=(const obs_t& obs1, const obs_t& obs2);
+
+bool operator==(const eph_t& eph1, const eph_t& eph2);
+bool operator!=(const eph_t& eph1, const eph_t& eph2);
+
+bool operator==(const geph_t& geph1, const geph_t& geph2);
+bool operator!=(const geph_t& geph1, const geph_t& geph2);
+
+bool operator==(const seph_t& seph1, const seph_t& seph2);
+bool operator!=(const seph_t& seph1, const seph_t& seph2);
+
+bool operator==(const peph_t& peph1, const peph_t& peph2);
+bool operator!=(const peph_t& peph1, const peph_t& peph2);
+
+bool operator==(const pclk_t& pclk1, const pclk_t& pclk2);
+bool operator!=(const pclk_t& pclk1, const pclk_t& pclk2);
+
+bool operator==(const alm_t& alm1, const alm_t& alm2);
+bool operator!=(const alm_t& alm1, const alm_t& alm2);
+
+bool operator==(const tec_t& tec1, const tec_t& tec2);
+bool operator!=(const tec_t& tec1, const tec_t& tec2);
+
+bool operator==(const fcbd_t& fcbd1, const fcbd_t& fcbd2);
+bool operator!=(const fcbd_t& fcbd1, const fcbd_t& fcbd2);
+
+bool operator==(const erp_t& erp1, const erp_t& erp2);
+bool operator!=(const erp_t& erp1, const erp_t& erp2);
+
+bool operator==(const nav_t& nav1, const nav_t& nav2);
+bool operator!=(const nav_t& nav1, const nav_t& nav2);
+
+#endif // INCLUDE_GNSS_UTILS_UTILS_UTILS_H_
diff --git a/internal/config.h.in b/internal/config.h.in
new file mode 100644
index 0000000000000000000000000000000000000000..90ad35cc6c35569f52a370d2c6624e0bed11c030
--- /dev/null
+++ b/internal/config.h.in
@@ -0,0 +1,10 @@
+#ifndef GNSS_UTILS_INTERNAL_CONFIG_H_
+#define GNSS_UTILS_INTERNAL_CONFIG_H_
+
+#cmakedefine _GNSS_UTILS_DEBUG
+
+#cmakedefine _GNSS_UTILS_TRACE
+
+#define _GNSS_UTILS_ROOT_DIR "${_GNSS_UTILS_ROOT_DIR}"
+
+#endif /* GNSS_UTILS_INTERNAL_CONFIG_H_ */
diff --git a/scripts/license_header_2022.txt b/scripts/license_header_2022.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3e36f9be0f73cf4dac051a75d96d72b372fdf464
--- /dev/null
+++ b/scripts/license_header_2022.txt
@@ -0,0 +1,17 @@
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
diff --git a/scripts/license_manager.sh b/scripts/license_manager.sh
new file mode 100755
index 0000000000000000000000000000000000000000..74f790a95133bb09f07b016ed55c3a7b67e2a9bc
--- /dev/null
+++ b/scripts/license_manager.sh
@@ -0,0 +1,144 @@
+#!/bin/bash
+
+# ------ WOLF license_manager script ------
+#
+# This script is used for managing the license headers of code files (.h, .c, .cpp, .hpp)
+# This file is automatically called by the CI in gitlab.
+
+line_start_mark="//--------LICENSE_START--------"
+line_end_mark="//--------LICENSE_END--------"
+
+#options
+tmp=false
+mode="none"
+path=""
+exclude_folder=""
+#recursive=true
+license=""
+
+for i in "$@"; do
+ case $i in
+ --temp)
+ tmp=true
+ shift # past argument=value
+ ;;
+ --path=*)
+ path="${i#*=}"
+ shift # past argument=value
+ ;;
+ --license-header=*)
+ license="${i#*=}"
+ shift # past argument=value
+ ;;
+ --add)
+ mode="add"
+ if [ $mode == "update" ]; then
+ echo "Error: Script cannot be called with both options: --update or --add"
+ exit 1
+ fi
+ shift # past argument=value
+ ;;
+ --update)
+ mode="update"
+ if [ $mode == "add" ]; then
+ echo "Error: Script cannot be called with both options: --update or --add"
+ exit 1
+ fi
+ shift # past argument=value
+ ;;
+ --exclude=*)
+ exclude_folder="${i#*=}"
+ shift # past argument=value
+ ;;
+ *)
+ # unknown option
+ ;;
+ esac
+done
+
+# check options
+if [ "$path" == "" ]; then
+ echo 'Please, provide the path to the folder containing the code with --path=/my/folder/code'
+ exit 1
+else
+ if [ -d "$path" ]; then
+ echo "Valid path: ${path}"
+ else
+ echo "Error: ${path} not found. Can not continue."
+ exit 1
+ fi
+fi
+
+if [ "$license" == "" ]; then
+ echo 'Error: Please, provide the path to the folder containing the code with --license-header=/my/license/header/file.txt'
+ exit 1
+else
+ if [ -f "$license" ]; then
+ echo "Valid license header file: ${license} containing:"
+ cat ${license}
+ else
+ echo "Error: License header file ${license} not found. Can not continue."
+ exit 1
+ fi
+fi
+
+if [ "$exclude_folder" == "" ]; then
+ echo "No folders will be excluded"
+else
+ if [ -d "${path}/${exclude_folder}" ]; then
+ echo "Valid remove folder path: ${path}/${exclude_folder}"
+ fi
+fi
+
+if [ $mode == "none" ]; then
+ echo "Error: Script should be called with one of the following options: --update or --add"
+ exit 1
+else
+ echo "mode: ${mode}"
+fi
+
+# PATH (AND tmp FOLDER)
+folder=$path
+if [ $tmp == true ]; then
+ echo "Creating temporary folder: ${path}_tmp"
+ mkdir -pv ${path}_tmp
+ cp -a $path/. ${path}_tmp
+ folder=${path}_tmp
+fi
+
+# LIST OF FILES TO BE ITERATED ON
+if [ "$exclude_folder" == "" ]; then
+ file_list=$(find $folder -name '*.c' -or -name '*.cpp' -or -name '*.h' -or -name '*.hpp')
+else
+ file_list=$(find $folder -path ${folder}/${exclude_folder} -prune -name '*.c' -or -name '*.cpp' -or -name '*.h' -or -name '*.hpp')
+fi
+
+# DETECT AND REMOVE EXISTING LICENSE
+if [ "$mode" == "update" ]; then
+ echo "Recursely removing license header from all files (.c, .cpp, .h, .hpp)"
+ for i in $file_list
+ do
+ if grep -Fxq ${line_start_mark} $i
+ then
+ echo " Removing license header from file ${i}"
+ line_start="$(grep -wn $line_start_mark ${i} | head -n 1 | cut -d: -f1)"
+ line_end="$(grep -wn $line_end_mark ${i} | head -n 1 | cut -d: -f1)"
+ #echo ${line_start}
+ #echo ${line_end}
+ awk -v m=$line_start -v n=$line_end 'm <= NR && NR <= n {next} {print}' $i > tmpfile && mv tmpfile $i
+ #cat $i
+ fi
+ done
+fi
+
+# ADD CONTENT OF license-file AT THE BEGINNING OF CODE FILES
+echo "Recursively adding license header to all files (.c, .cpp, .h, .hpp)"
+for i in $file_list
+do
+ if grep -Fxq ${line_start_mark} $i; then
+ echo "skippping ${i}"
+ else
+ ( echo ${line_start_mark}$'\n//'; cat ${license}; echo $'//\n'${line_end_mark}; cat $i ) > temp_file
+ mv temp_file $i
+ fi
+done
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
deleted file mode 100644
index cc0421b22732bbc4d4f41d7d6415b8bdb0175d6f..0000000000000000000000000000000000000000
--- a/src/CMakeLists.txt
+++ /dev/null
@@ -1,37 +0,0 @@
-# driver source files
-SET(sources gnss_utils.cpp observation.cpp)
-
-# application header files
-SET(headers ../include/gnss_utils.h ../include/observation.h)
-
-# RTKLIB
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindRTKLIB.cmake)
-
-# Eigen #######
-FIND_PACKAGE(Eigen3 REQUIRED)
-
-# Boost #######
-set(Boost_USE_STATIC_LIBS OFF)
-set(Boost_USE_MULTITHREADED OFF)
-set(Boost_USE_STATIC_RUNTIME OFF)
-FIND_PACKAGE(Boost REQUIRED)
-
-# Adding libraries' directories
-link_directories(/usr/lib/x86_64-linux-gnu/)
-
-# Adding include directories
-INCLUDE_DIRECTORIES(. ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${RTK_LIB_PATH})
-
-# create the shared library
-ADD_LIBRARY(gnss_utils SHARED ${sources})
-TARGET_LINK_LIBRARIES(gnss_utils ${Boost_LIBRARIES})
-
-# Installing
-INSTALL(TARGETS gnss_utils
- RUNTIME DESTINATION bin
- LIBRARY DESTINATION /usr/local/lib
- ARCHIVE DESTINATION lib)
-INSTALL(FILES ${headers} DESTINATION include/gnss_utils)
-# INSTALL(FILES ../gnss_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
-INSTALL(FILES ../Findgnss_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
-ADD_SUBDIRECTORY(examples)
diff --git a/src/examples/gnss_utils_test.cpp b/src/examples/gnss_utils_test.cpp
index 8fd4243b068ee72957dfd3bf6a81682e9b367b6a..4689c4a7a89bf12afea1620e3d2bc86d0aa0f58b 100644
--- a/src/examples/gnss_utils_test.cpp
+++ b/src/examples/gnss_utils_test.cpp
@@ -1,40 +1,126 @@
-#include "../../include/gnss_utils.h"
-#include "../../include/observation.h"
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+#include "gnss_utils/utils/transformations.h"
+#include "gnss_utils/utils/utils.h"
+
+#include <typeinfo>
#include <iostream>
-using namespace GNSSUtils;
+extern "C" {
+#include "../deps/RTKLIB/src/rinex.c"
+}
+
+using namespace GnssUtils;
-int main(int argc, char *argv[])
+int main(int argc, char* argv[])
{
// header
std::cout << "--------------------------" << std::endl;
std::cout << "GNSS Utils Library Example" << std::endl;
std::cout << "--------------------------" << std::endl;
- // create a Receiver object
- GNSSUtils::Receiver test_rcv;
-
- // create a Observation object
- gtime_t ts={0};
- unsigned char q=1, w=2, e=3;
- std::vector<unsigned char> vu({q, w, e});
- std::vector<double> vd({1., 2., 3.});
- std::vector<float> vf({1., 2., 3.});
- Observation test_obs(ts, 1, 1, vu, vu, vu, vd, vd, vf);
-
- // Display current Observation
- std::cout << "Current observation (sat): " << "(" << test_obs.getSat() << ")" << std::endl;
- std::cout << "Current observation (SNR): " << "(" ;
-
-
-
- for (unsigned char i=0; i < 3; ++i)
- std::cout << test_obs.getSNR()[i] << ", ";
- std::cout << ")" << std::endl;
-
-
- obsd_t obs = test_obs.convert2rtk();
- std::cout << "Observation SNR in RTKLIB format: " << "(";
- for (unsigned char i=0; i < 3; ++i)
- std::cout << obs.SNR[i] << ", ";
- std::cout << ")" << std::endl;
+
+ // create Observations object
+ Observations observations;
+
+ // create Navigation object
+ Navigation navigation;
+
+ gtime_t t_start{ 0, 0 }; // no limit
+ gtime_t t_end{ 0, 0 }; // no limit
+ double dt = 0.0; // no limit
+ const char* opt = ""; // only GPS | GPS+GAL: "-SYS=G,L" | ALL: ""
+
+ // load observations from RINEX file
+ std::vector<Observations> obs_rinex =
+ loadObservationsFromRinex("../src/examples/sample_data.obs", t_start, t_end, dt, opt);
+ observations = obs_rinex[0];
+ observations.print();
+
+ // RTKLIB trace
+ char str_file[80];
+ snprintf(str_file, sizeof str_file, "../src/examples/trace");
+ tracelevel(5);
+ traceopen(str_file);
+
+ // load navigation from RINEX file
+ navigation.loadFromRinex("../src/examples/sample_data.nav", t_start, t_end, dt, opt);
+ navigation.print();
+
+ // Trace close
+ // traceclose();
+
+ /* Set processing options */
+
+ /* header */
+ std::cout << "------------------" << std::endl;
+ std::cout << "Processing options" << std::endl;
+ std::cout << "------------------" << std::endl;
+
+ prcopt_t prcopt = prcopt_default;
+ prcopt.mode = PMODE_SINGLE;
+ prcopt.soltype = 0;
+ prcopt.nf = 1;
+ prcopt.navsys = SYS_GPS;
+ prcopt.elmin = 0.525; // 60 degrees = 1.05 rad
+ prcopt.sateph = EPHOPT_BRDC;
+ prcopt.ionoopt = IONOOPT_OFF;
+ prcopt.tropopt = TROPOPT_OFF;
+ prcopt.dynamics = 0;
+ prcopt.tidecorr = 0;
+ prcopt.sbascorr = SBSOPT_FCORR;
+ prcopt.ru[0] = 0; // 4789374.0336;
+ prcopt.ru[1] = 0; // 177048.3292;
+ prcopt.ru[2] = 0; // 4194542.6444;
+
+ std::cout << "Processing options defined" << std::endl;
+
+ Eigen::Vector3d lla_gt(41.383293114 * M_PI / 180.0, 2.116101115 * M_PI / 180.0, -91.6641);
+
+ // Compute spp
+
+ /* header */
+ std::cout << "-----------" << std::endl;
+ std::cout << "pntpos call" << std::endl;
+ std::cout << "-----------" << std::endl;
+
+ int stat;
+
+ sol_t solb = { { 0 } };
+
+ char msg[128] = "";
+
+ stat = pntpos(observations.data(), observations.size(), &navigation.getNavigation(), &prcopt, &solb, NULL, NULL,
+ msg);
+
+ std::cout << "msg: " << msg << std::endl;
+ std::cout << "sol.stat: " << int(solb.stat) << std::endl;
+ std::cout << "Position: " << solb.rr[0] << ", " << solb.rr[1] << ", " << solb.rr[2] << std::endl;
+ std::cout << "Position (GT): " << latLonAltToEcef(lla_gt).transpose() << std::endl;
+ std::cout << "Position LLA: " << ecefToLatLonAlt(Eigen::Vector3d(solb.rr[0], solb.rr[1],
+ solb.rr[2])).transpose()
+ << std::endl;
+ std::cout << "Position LLA (GT): " << lla_gt.transpose() << std::endl;
+
+ // traceclose();
}
diff --git a/src/examples/sample_data.nav b/src/examples/sample_data.nav
new file mode 100644
index 0000000000000000000000000000000000000000..5c4c418dc876fd45e7fddca388b4cc98b0202441
--- /dev/null
+++ b/src/examples/sample_data.nav
@@ -0,0 +1,406 @@
+ 3.04 N: GNSS NAV DATA M: MIXED RINEX VERSION / TYPE
+sbf2rin-13.4.3 20200205 113410 UTC PGM / RUN BY / DATE
+GPSA 9.3132E-09 -1.4901E-08 -5.9605E-08 1.1921E-07 IONOSPHERIC CORR
+GPSB 9.6256E+04 -1.4746E+05 -1.3107E+05 9.1750E+05 IONOSPHERIC CORR
+GAL 2.9250E+01 3.8281E-01 3.2959E-03 0.0000E+00 IONOSPHERIC CORR
+GPUT -9.3132257462E-10 8.881784197E-16 503808 2083 TIME SYSTEM CORR
+GAUT 9.3132257462E-10 0.000000000E+00 345600 2083 TIME SYSTEM CORR
+GAGP 1.2514647096E-09-5.329070518E-15 345600 2083 TIME SYSTEM CORR
+ 18 LEAP SECONDS
+ END OF HEADER
+G02 2019 12 12 12 00 00-3.648423589766E-04-7.503331289627E-12 0.000000000000E+00
+ 4.700000000000E+01-1.208437500000E+02 4.378039505776E-09-1.891689129337E+00
+ -6.053596735001E-06 1.953727600630E-02 9.275972843170E-06 5.153594808578E+03
+ 3.888000000000E+05 3.371387720108E-07-2.768163608668E-01 2.346932888031E-07
+ 9.575379914494E-01 1.915312500000E+02-1.691294764827E+00-7.654961717025E-09
+ 2.000083311498E-11 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00-1.769512891769E-08 4.700000000000E+01
+ 3.868920000000E+05 4.000000000000E+00
+G04 2019 12 12 12 00 00-1.993030309677E-05-5.002220859751E-12 0.000000000000E+00
+ 1.570000000000E+02-1.387500000000E+01 4.959135139313E-09-2.339868507292E+00
+ -8.419156074524E-07 1.249722088687E-03 5.329027771950E-06 5.150576126099E+03
+ 3.888000000000E+05 5.587935447693E-09 1.915372002909E+00 1.303851604462E-08
+ 9.594040575671E-01 2.769062500000E+02-1.281079249179E+00-8.132124449911E-09
+ -3.685867816904E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 4.096000000000E+03 6.300000000000E+01-4.190951585770E-09 6.690000000000E+02
+ 3.873960000000E+05 4.000000000000E+00
+G06 2019 12 12 12 00 00-1.512947492301E-04-1.114131009672E-11 0.000000000000E+00
+ 1.000000000000E+02-1.239062500000E+02 3.927663602954E-09-1.824134113525E+00
+ -6.347894668579E-06 1.658447668888E-03 9.786337614059E-06 5.153719285965E+03
+ 3.888000000000E+05-1.490116119385E-08-2.122724576342E-01 5.401670932770E-08
+ 9.781077241470E-01 2.010000000000E+02-1.200768673201E+00-7.640318249923E-09
+ 1.214336296267E-11 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00 4.656612873077E-09 1.000000000000E+02
+ 3.858180000000E+05 4.000000000000E+00
+G07 2019 12 12 12 00 00-1.683332957327E-04-8.185452315956E-12 0.000000000000E+00
+ 5.700000000000E+01 3.909375000000E+01 4.250534194668E-09-2.882882750792E+00
+ 1.888722181320E-06 1.320131728426E-02 1.172721385956E-05 5.153718780518E+03
+ 3.888000000000E+05-7.450580596924E-09 2.943673467473E+00-1.341104507446E-07
+ 9.545190186113E-01 1.482812500000E+02-2.422380694519E+00-7.540671242082E-09
+ -4.639478967207E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00-1.117587089539E-08 5.700000000000E+01
+ 3.858180000000E+05 4.000000000000E+00
+G09 2019 12 12 12 00 00-1.062308438122E-04-9.436007530894E-12 0.000000000000E+00
+ 7.800000000000E+01-1.206250000000E+01 5.168786729286E-09 4.992301981873E-01
+ -7.078051567078E-07 1.692383317277E-03 5.021691322327E-06 5.153534730911E+03
+ 3.888000000000E+05-2.793967723846E-08 1.868752621939E+00 1.490116119385E-08
+ 9.520568016730E-01 2.776875000000E+02 1.684705661839E+00-8.283559329210E-09
+ -2.914407111040E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00 1.396983861923E-09 7.800000000000E+01
+ 3.857520000000E+05 4.000000000000E+00
+G13 2019 12 12 12 00 00-2.714386209846E-05 2.387423592154E-12 0.000000000000E+00
+ 8.700000000000E+01-1.987500000000E+01 4.779841956746E-09-8.793798521920E-01
+ -1.072883605957E-06 4.130274290219E-03 5.709007382393E-06 5.153656044006E+03
+ 3.888000000000E+05-6.332993507385E-08 2.009100851183E+00 9.313225746155E-08
+ 9.674158381471E-01 2.706562500000E+02 1.183260297004E+00-8.083193840326E-09
+ -5.078782980268E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00-1.117587089539E-08 8.700000000000E+01
+ 3.870420000000E+05 4.000000000000E+00
+G23 2019 12 12 12 00 00-1.500290818512E-04 2.387423592154E-12 0.000000000000E+00
+ 3.000000000000E+01-1.009375000000E+01 5.335222233421E-09-1.452196357053E+00
+ -4.973262548447E-07 1.326873130165E-02 4.552304744720E-06 5.153694892883E+03
+ 3.888000000000E+05 2.980232238770E-07 1.861752114203E+00 4.470348358154E-08
+ 9.428683609718E-01 2.778750000000E+02-2.215284453592E+00-8.261415549690E-09
+ -2.478674675321E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00-2.048909664154E-08 3.000000000000E+01
+ 3.858420000000E+05 4.000000000000E+00
+G30 2019 12 12 12 00 00-1.042019575834E-04-8.640199666843E-12 0.000000000000E+00
+ 2.500000000000E+01 4.368750000000E+01 4.663408535398E-09-2.906933186921E+00
+ 2.210959792137E-06 4.164319136180E-03 1.142919063568E-05 5.153722436905E+03
+ 3.888000000000E+05-2.793967723846E-08 2.968865819418E+00-3.725290298462E-09
+ 9.399056183160E-01 1.480312500000E+02-2.921787544453E+00-7.888185717455E-09
+ -4.778770483544E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00 3.725290298462E-09 2.500000000000E+01
+ 3.858180000000E+05 4.000000000000E+00
+R03 2019 12 12 11 15 00 6.369315087795E-06 0.000000000000E+00 3.858000000000E+05
+ 9.652274414063E+03 3.457994461060E-01 9.313225746155E-10 0.000000000000E+00
+ 1.708528710938E+04 2.202743530273E+00-1.862645149231E-09 5.000000000000E+00
+ 1.629986035156E+04-2.522974967957E+00-2.793967723846E-09 0.000000000000E+00
+R03 2019 12 12 11 45 00 6.371177732944E-06 0.000000000000E+00 3.870000000000E+05
+ 1.043946044922E+04 4.914197921753E-01 9.313225746155E-10 0.000000000000E+00
+ 2.039817236328E+04 1.453030586243E+00-1.862645149231E-09 5.000000000000E+00
+ 1.118693066406E+04-3.121288299561E+00-1.862645149231E-09 0.000000000000E+00
+R05 2019 12 12 11 15 00 4.108343273401E-05 9.094947017729E-13 3.862800000000E+05
+ 5.985222167969E+03 1.195173263550E-01-1.862645149231E-09 0.000000000000E+00
+ -1.828012353516E+04 2.295486450195E+00 1.862645149231E-09 1.000000000000E+00
+ 1.676849218750E+04 2.457766532898E+00-1.862645149231E-09 0.000000000000E+00
+R05 2019 12 12 11 45 00 4.108529537916E-05 9.094947017729E-13 3.871800000000E+05
+ 6.598400390625E+03 5.781021118164E-01-9.313225746155E-10 0.000000000000E+00
+ -1.370180371094E+04 2.745733261108E+00 9.313225746155E-10 1.000000000000E+00
+ 2.048785058594E+04 1.648019790649E+00-1.862645149231E-09 0.000000000000E+00
+R14 2019 12 12 11 15 00 4.597380757332E-05 0.000000000000E+00 3.858000000000E+05
+ 2.135816357422E+04-6.865911483765E-01 3.725290298462E-09 0.000000000000E+00
+ 1.261080761719E+04-4.694700241089E-01-0.000000000000E+00-7.000000000000E+00
+ 5.991423828125E+03 3.442845344543E+00-2.793967723846E-09 0.000000000000E+00
+R14 2019 12 12 11 45 00 4.597473889589E-05 0.000000000000E+00 3.870000000000E+05
+ 1.936734765625E+04-1.519350051880E+00 1.862645149231E-09 0.000000000000E+00
+ 1.162212011719E+04-5.869359970093E-01-9.313225746155E-10-7.000000000000E+00
+ 1.187716162109E+04 3.054467201233E+00-2.793967723846E-09 0.000000000000E+00
+R15 2019 12 12 11 15 00 1.051910221577E-04 0.000000000000E+00 3.860100000000E+05
+ 2.249286962891E+04 1.470921516418E+00 5.587935447693E-09 0.000000000000E+00
+ 4.179755859375E+03 4.950771331787E-01 9.313225746155E-10 0.000000000000E+00
+ -1.125261865234E+04 3.118181228638E+00-1.862645149231E-09 0.000000000000E+00
+R15 2019 12 12 11 45 00 1.051910221577E-04 0.000000000000E+00 3.870000000000E+05
+ 2.451276953125E+04 7.425718307495E-01 4.656612873077E-09 0.000000000000E+00
+ 4.643450683594E+03 4.951381683350E-02 0.000000000000E+00 0.000000000000E+00
+ -5.276882812500E+03 3.478320121765E+00-2.793967723846E-09 0.000000000000E+00
+R19 2019 12 12 11 15 00-6.853323429823E-05-1.818989403546E-12 3.859500000000E+05
+ 2.008951708984E+04 2.060537338257E-01 2.793967723846E-09 0.000000000000E+00
+ -1.541425244141E+04-4.377956390381E-01 2.793967723846E-09 3.000000000000E+00
+ 3.049126953125E+03-3.578741073608E+00-1.862645149231E-09 0.000000000000E+00
+R19 2019 12 12 11 45 00-6.853695958853E-05-1.818989403546E-12 3.870000000000E+05
+ 1.978871044922E+04-5.135507583618E-01 3.725290298462E-09 0.000000000000E+00
+ -1.571828076172E+04 1.333026885986E-01 1.862645149231E-09 3.000000000000E+00
+ -3.427280273438E+03-3.570507049561E+00-1.862645149231E-09 0.000000000000E+00
+R13 2019 12 12 11 45 00-3.215298056602E-05-0.000000000000E+00 3.876000000000E+05
+ 3.100402832031E+03-2.939124107361E+00-1.862645149231E-09 0.000000000000E+00
+ 1.186301953125E+04-8.695030212402E-01-9.313225746155E-10-2.000000000000E+00
+ 2.235348193359E+04 8.685455322266E-01-1.862645149231E-09 0.000000000000E+00
+R20 2019 12 12 11 45 00-3.986386582255E-04-0.000000000000E+00 3.877800000000E+05
+ 9.837636718750E+03 1.428291320801E+00-0.000000000000E+00 0.000000000000E+00
+ -1.795626220703E+04-1.534766197205E+00 1.862645149231E-09 2.000000000000E+00
+ 1.525312060547E+04-2.726587295532E+00-1.862645149231E-09 0.000000000000E+00
+E01 2019 12 12 11 10 00-7.507699192502E-04-7.986500349944E-12 0.000000000000E+00
+ 3.000000000000E+00 2.013125000000E+02 2.449744898851E-09 7.981291655576E-01
+ 9.473413228989E-06 1.731918891892E-04 9.533017873764E-06 5.440622966766E+03
+ 3.858000000000E+05 3.166496753693E-08-2.605880888987E+00-1.117587089539E-08
+ 9.855193908613E-01 1.463750000000E+02-1.069210219000E+00-5.272719629937E-09
+ 1.914365455291E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09-1.862645149231E-09
+ 3.864650000000E+05
+E01 2019 12 12 11 20 00-7.507746340707E-04-7.972289495228E-12 0.000000000000E+00
+ 4.000000000000E+00 2.002500000000E+02 2.449744898851E-09 8.736282654153E-01
+ 9.419396519661E-06 1.731686061248E-04 9.588897228241E-06 5.440622581482E+03
+ 3.864000000000E+05 3.725290298462E-08-2.605884063520E+00-1.862645149231E-09
+ 9.855195488564E-01 1.452500000000E+02-1.070324887968E+00-5.265933632987E-09
+ 1.896507568581E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09-1.862645149231E-09
+ 3.870650000000E+05
+E09 2019 12 12 11 10 00 6.223833654076E-03-1.216449163621E-11 0.000000000000E+00
+ 3.000000000000E+00 7.218750000000E+00 3.823373544569E-09-3.442271110123E-02
+ 3.892928361893E-07 2.964780433103E-04 3.403052687645E-06 5.440603132248E+03
+ 3.858000000000E+05-9.313225746155E-09 1.575307486314E+00-6.146728992462E-08
+ 9.586413959478E-01 2.625000000000E+02 2.794206052069E-01-5.815242228181E-09
+ 8.571785620706E-12 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-4.656612873077E-10-6.984919309616E-10
+ 3.869030000000E+05
+E12 2019 12 12 10 40 00 6.055364210624E-03-1.870148480521E-11 0.000000000000E+00
+ 0.000000000000E+00-2.075312500000E+02 2.603322724555E-09-2.414195903395E+00
+ -9.659677743912E-06 2.996901748702E-04 6.495043635368E-06 5.440614477158E+03
+ 3.840000000000E+05-2.421438694000E-08-5.219517721978E-01 5.774199962616E-08
+ 9.864495053986E-01 2.151250000000E+02-9.348391204646E-01-5.376652530588E-09
+ -2.257236880119E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.327134668827E-08-1.327134668827E-08
+ 3.858050000000E+05
+E19 2019 12 12 11 00 00-2.469634637237E-06 2.273736754432E-13 0.000000000000E+00
+ 2.000000000000E+00 1.103125000000E+01 3.765871149364E-09-1.685770007887E-01
+ 4.824250936508E-07 1.212444622070E-04 3.688037395477E-06 5.440600940704E+03
+ 3.852000000000E+05-5.401670932770E-08 1.580615112564E+00 5.215406417847E-08
+ 9.583897608719E-01 2.595937500000E+02 1.996512379669E+00-5.770954669140E-09
+ 1.160762636137E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09-5.820766091347E-09
+ 3.858660000000E+05
+E19 2019 12 12 11 10 00-2.469576429576E-06 2.273736754432E-13 0.000000000000E+00
+ 3.000000000000E+00 1.168750000000E+01 3.765156833895E-09-9.388288915917E-02
+ 5.066394805908E-07 1.211694907397E-04 3.693625330925E-06 5.440600774765E+03
+ 3.858000000000E+05-5.960464477539E-08 1.580611582543E+00 4.097819328308E-08
+ 9.583898398694E-01 2.594687500000E+02 1.996204499781E+00-5.764168672191E-09
+ 1.207193141583E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09-5.820766091347E-09
+ 3.864650000000E+05
+E21 2019 12 12 11 10 00-5.709171527997E-04-2.131628207280E-12 0.000000000000E+00
+ 3.000000000000E+00 2.180312500000E+02 2.333311477503E-09 1.537835498027E+00
+ 1.022964715958E-05 9.841390419751E-05 9.685754776001E-06 5.440636165619E+03
+ 3.858000000000E+05-2.980232238770E-08-2.608584029515E+00 4.842877388000E-08
+ 9.850865499746E-01 1.424062500000E+02-1.025413854454E+00-5.217717338871E-09
+ 1.871506527187E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 2.328306436539E-10
+ 3.867350000000E+05
+E21 2019 12 12 11 20 00-5.709183751605E-04-2.131628207280E-12 0.000000000000E+00
+ 4.000000000000E+00 2.213125000000E+02 2.317953694933E-09 1.607657241034E+00
+ 1.038052141666E-05 9.810912888497E-05 9.816139936447E-06 5.440637243271E+03
+ 3.864000000000E+05-4.097819328308E-08-2.608587130902E+00 4.470348358154E-08
+ 9.850867050439E-01 1.395312500000E+02-1.020851268976E+00-5.213788603794E-09
+ 1.800074980348E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 2.328306436539E-10
+ 3.870650000000E+05
+E01 2019 12 12 11 10 00-7.507689879276E-04-7.986500349944E-12 0.000000000000E+00
+ 3.000000000000E+00 2.013125000000E+02 2.449744898851E-09 7.981291655576E-01
+ 9.473413228989E-06 1.731918891892E-04 9.533017873764E-06 5.440622966766E+03
+ 3.858000000000E+05 3.166496753693E-08-2.605880888987E+00-1.117587089539E-08
+ 9.855193908613E-01 1.463750000000E+02-1.069210219000E+00-5.272719629937E-09
+ 1.914365455291E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09 0.000000000000E+00
+ 3.865400000000E+05
+E01 2019 12 12 11 20 00-7.507736445405E-04-7.972289495228E-12 0.000000000000E+00
+ 4.000000000000E+00 2.002500000000E+02 2.449744898851E-09 8.736282654153E-01
+ 9.419396519661E-06 1.731686061248E-04 9.588897228241E-06 5.440622581482E+03
+ 3.864000000000E+05 3.725290298462E-08-2.605884063520E+00-1.862645149231E-09
+ 9.855195488564E-01 1.452500000000E+02-1.070324887968E+00-5.265933632987E-09
+ 1.896507568581E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09 0.000000000000E+00
+ 3.871400000000E+05
+E12 2019 12 12 10 40 00 6.055364909116E-03-1.871569565992E-11 0.000000000000E+00
+ 0.000000000000E+00-2.075312500000E+02 2.603322724555E-09-2.414195903395E+00
+ -9.659677743912E-06 2.996901748702E-04 6.495043635368E-06 5.440614477158E+03
+ 3.840000000000E+05-2.421438694000E-08-5.219517721978E-01 5.774199962616E-08
+ 9.864495053986E-01 2.151250000000E+02-9.348391204646E-01-5.376652530588E-09
+ -2.257236880119E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.327134668827E-08 0.000000000000E+00
+ 3.858400000000E+05
+E19 2019 12 12 11 00 00-2.468528691679E-06 2.273736754432E-13 0.000000000000E+00
+ 2.000000000000E+00 1.103125000000E+01 3.765871149364E-09-1.685770007887E-01
+ 4.824250936508E-07 1.212444622070E-04 3.688037395477E-06 5.440600940704E+03
+ 3.852000000000E+05-5.401670932770E-08 1.580615112564E+00 5.215406417847E-08
+ 9.583897608719E-01 2.595937500000E+02 1.996512379669E+00-5.770954669140E-09
+ 1.160762636137E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09 0.000000000000E+00
+ 3.859400000000E+05
+E19 2019 12 12 11 10 00-2.468470484018E-06 2.273736754432E-13 0.000000000000E+00
+ 3.000000000000E+00 1.168750000000E+01 3.765156833895E-09-9.388288915917E-02
+ 5.066394805908E-07 1.211694907397E-04 3.693625330925E-06 5.440600774765E+03
+ 3.858000000000E+05-5.960464477539E-08 1.580611582543E+00 4.097819328308E-08
+ 9.583898398694E-01 2.594687500000E+02 1.996204499781E+00-5.764168672191E-09
+ 1.207193141583E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09 0.000000000000E+00
+ 3.865400000000E+05
+E21 2019 12 12 11 10 00-5.709161632694E-04-2.117417352565E-12 0.000000000000E+00
+ 3.000000000000E+00 2.180312500000E+02 2.333311477503E-09 1.537835498027E+00
+ 1.022964715958E-05 9.841390419751E-05 9.685754776001E-06 5.440636165619E+03
+ 3.858000000000E+05-2.980232238770E-08-2.608584029515E+00 4.842877388000E-08
+ 9.850865499746E-01 1.424062500000E+02-1.025413854454E+00-5.217717338871E-09
+ 1.871506527187E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 0.000000000000E+00
+ 3.867900000000E+05
+E21 2019 12 12 11 20 00-5.709173856303E-04-2.131628207280E-12 0.000000000000E+00
+ 4.000000000000E+00 2.213125000000E+02 2.317953694933E-09 1.607657241034E+00
+ 1.038052141666E-05 9.810912888497E-05 9.816139936447E-06 5.440637243271E+03
+ 3.864000000000E+05-4.097819328308E-08-2.608587130902E+00 4.470348358154E-08
+ 9.850867050439E-01 1.395312500000E+02-1.020851268976E+00-5.213788603794E-09
+ 1.800074980348E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 0.000000000000E+00
+ 3.871400000000E+05
+E04 2019 12 12 11 10 00-4.236891982146E-04-7.531752999057E-12 0.000000000000E+00
+ 3.000000000000E+00 9.781250000000E+00 3.935521073107E-09-4.009111796513E-01
+ 5.532056093216E-07 8.811207953840E-05 3.596767783165E-06 5.440604427338E+03
+ 3.858000000000E+05 9.499490261078E-08 1.582511472789E+00 3.166496753693E-08
+ 9.526713574660E-01 2.577187500000E+02 1.440283619102E+00-5.908817554540E-09
+ 7.071723137082E-11 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.355104804039E-09-5.820766091347E-09
+ 3.875670000000E+05
+E04 2019 12 12 11 10 00-4.236878594384E-04-7.531752999057E-12 0.000000000000E+00
+ 3.000000000000E+00 9.781250000000E+00 3.935521073107E-09-4.009111796513E-01
+ 5.532056093216E-07 8.811207953840E-05 3.596767783165E-06 5.440604427338E+03
+ 3.858000000000E+05 9.499490261078E-08 1.582511472789E+00 3.166496753693E-08
+ 9.526713574660E-01 2.577187500000E+02 1.440283619102E+00-5.908817554540E-09
+ 7.071723137082E-11 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.355104804039E-09 0.000000000000E+00
+ 3.875900000000E+05
+E11 2019 12 12 11 00 00 3.295102505945E-04 1.526245796413E-10 5.204170427930E-18
+ 2.000000000000E+00-2.123750000000E+02 2.665468170305E-09 2.480654590844E+00
+ -9.858980774879E-06 1.651302445680E-04 7.150694727898E-06 5.440609029770E+03
+ 3.852000000000E+05-3.352761268616E-08-5.219456059981E-01 2.048909664154E-08
+ 9.864349859366E-01 1.975937500000E+02-2.012781103955E-01-5.400939256513E-09
+ -2.103659054415E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.583248376846E-08-1.699663698673E-08
+ 3.876330000000E+05
+E21 2019 12 12 11 30 00-5.709196557291E-04-2.131628207280E-12 0.000000000000E+00
+ 5.000000000000E+00 2.240625000000E+02 2.303667385565E-09 1.677767938158E+00
+ 1.050904393196E-05 9.777839295566E-05 9.970739483833E-06 5.440638303757E+03
+ 3.870000000000E+05-5.401670932770E-08-2.608590226436E+00 3.725290298462E-08
+ 9.850868571874E-01 1.362812500000E+02-1.016577701982E+00-5.210574184187E-09
+ 1.721500278825E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 2.328306436539E-10
+ 3.876650000000E+05
+E11 2019 12 12 11 00 00 3.295124042779E-04 1.526387904960E-10 5.204170427930E-18
+ 2.000000000000E+00-2.123750000000E+02 2.665468170305E-09 2.480654590844E+00
+ -9.858980774879E-06 1.651302445680E-04 7.150694727898E-06 5.440609029770E+03
+ 3.852000000000E+05-3.352761268616E-08-5.219456059981E-01 2.048909664154E-08
+ 9.864349859366E-01 1.975937500000E+02-2.012781103955E-01-5.400939256513E-09
+ -2.103659054415E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.583248376846E-08 0.000000000000E+00
+ 3.876800000000E+05
+E01 2019 12 12 11 30 00-7.507792906836E-04-7.972289495228E-12 0.000000000000E+00
+ 5.000000000000E+00 1.994687500000E+02 2.447601952446E-09 9.486982138657E-01
+ 9.378418326378E-06 1.731649972498E-04 9.616836905479E-06 5.440622346878E+03
+ 3.870000000000E+05 3.911554813385E-08-2.605887229274E+00 7.450580596924E-09
+ 9.855197039257E-01 1.446875000000E+02-1.071010335308E+00-5.259504793772E-09
+ 1.871506527187E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09-1.862645149231E-09
+ 3.877250000000E+05
+E21 2019 12 12 11 30 00-5.709186661988E-04-2.117417352565E-12 0.000000000000E+00
+ 5.000000000000E+00 2.240625000000E+02 2.303667385565E-09 1.677767938158E+00
+ 1.050904393196E-05 9.777839295566E-05 9.970739483833E-06 5.440638303757E+03
+ 3.870000000000E+05-5.401670932770E-08-2.608590226436E+00 3.725290298462E-08
+ 9.850868571874E-01 1.362812500000E+02-1.016577701982E+00-5.210574184187E-09
+ 1.721500278825E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 0.000000000000E+00
+ 3.877400000000E+05
+E01 2019 12 12 11 30 00-7.507783011533E-04-7.972289495228E-12 0.000000000000E+00
+ 5.000000000000E+00 1.994687500000E+02 2.447601952446E-09 9.486982138657E-01
+ 9.378418326378E-06 1.731649972498E-04 9.616836905479E-06 5.440622346878E+03
+ 3.870000000000E+05 3.911554813385E-08-2.605887229274E+00 7.450580596924E-09
+ 9.855197039257E-01 1.446875000000E+02-1.071010335308E+00-5.259504793772E-09
+ 1.871506527187E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09 0.000000000000E+00
+ 3.878200000000E+05
+E36 2019 12 12 10 30 00 6.253067986108E-04-4.803268893738E-12 0.000000000000E+00
+ 1.270000000000E+02-2.396562500000E+02 2.521890761159E-09 3.060908164528E+00
+ -1.121312379837E-05 3.076605498791E-04 7.288530468941E-06 5.440609262466E+03
+ 3.834000000000E+05 2.980232238770E-08-5.264936383676E-01 5.587935447693E-08
+ 9.896539850266E-01 1.944062500000E+02-1.782833699345E+00-5.428440402046E-09
+ -2.457245211269E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 3.725290298462E-09 4.423782229424E-09
+ 3.878450000000E+05
+S36 2019 12 12 11 36 48 0.000000000000E+00 0.000000000000E+00 3.874820000000E+05
+ 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 2.530000000000E+02
+S25 2019 12 11 23 59 44 0.000000000000E+00 0.000000000000E+00 3.874840000000E+05
+ 4.053076976000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ -1.162201120000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00
+S23 2019 12 12 11 37 04 0.000000000000E+00 0.000000000000E+00 3.874940000000E+05
+ 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.460000000000E+02
+S36 2019 12 12 11 38 56 0.000000000000E+00 0.000000000000E+00 3.875460000000E+05
+ 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 2.540000000000E+02
+S23 2019 12 12 11 39 12 0.000000000000E+00 0.000000000000E+00 3.875580000000E+05
+ 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.470000000000E+02
+S23 2019 12 12 11 41 20 0.000000000000E+00 0.000000000000E+00 3.876860000000E+05
+ 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.480000000000E+02
+S36 2019 12 12 11 41 04 0.000000000000E+00 0.000000000000E+00 3.877380000000E+05
+ 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 2.550000000000E+02
+S36 2019 12 12 11 43 12 0.000000000000E+00 0.000000000000E+00 3.878020000000E+05
+ 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00
+S23 2019 12 12 11 43 28 0.000000000000E+00 0.000000000000E+00 3.878780000000E+05
+ 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.490000000000E+02
+C05 2019 12 12 11 00 00-2.244751667604E-04-5.808153957787E-11 0.000000000000E+00
+ 1.000000000000E+00-4.986718750000E+02 1.785788670980E-12-5.169766818568E-01
+ -1.630326732993E-05 6.773804780096E-04 2.061109989882E-05 6.493346771240E+03
+ 3.852000000000E+05 1.271255314350E-07-1.766797704845E-01 3.259629011154E-09
+ 1.180676987021E-01-6.241562500000E+02-1.608095995036E+00 1.192549674481E-09
+ 7.575315542299E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00-8.000000000000E-10-9.200000000000E-09
+ 3.858276000000E+05 0.000000000000E+00
+C10 2019 12 12 11 00 00-4.537283675745E-04-3.685762806072E-11 0.000000000000E+00
+ 1.000000000000E+00 9.611718750000E+02 9.743262988869E-10-3.040647920928E+00
+ 3.135204315186E-05 6.156359100714E-03 7.578637450933E-06 6.493785018921E+03
+ 3.852000000000E+05-1.676380634308E-08-2.379959881122E+00-4.004687070847E-08
+ 9.058375010021E-01-1.493750000000E+01-2.570309406835E+00-2.115088101909E-09
+ 4.782342060886E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00 5.000000000000E-09 2.400000000000E-09
+ 3.858180000000E+05 0.000000000000E+00
+C23 2019 12 12 11 00 00-8.696540025994E-04 2.136069099379E-12 0.000000000000E+00
+ 1.000000000000E+00 1.034062500000E+02 3.498002848716E-09 1.631067082891E+00
+ 5.179084837437E-06 2.014992060140E-04 1.188227906823E-05 5.282628129959E+03
+ 3.852000000000E+05 3.725290298462E-08-3.016985033742E+00 6.565824151039E-08
+ 9.546959600159E-01 1.175156250000E+02-1.126531243634E+00-6.488127399406E-09
+ -1.864363372504E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00 2.430000000000E-08 2.430000000000E-08
+ 3.858180000000E+05 1.000000000000E+00
+C30 2019 12 12 11 00 00 2.388220746070E-04 6.104450278599E-12 0.000000000000E+00
+ 1.000000000000E+00-1.393437500000E+02 3.927663602954E-09-2.732742750025E+00
+ -6.837770342827E-06 3.649030113593E-04 1.750886440277E-06 5.282614929199E+03
+ 3.852000000000E+05-3.864988684654E-08-9.521190897989E-01-5.587935447693E-09
+ 9.644793109832E-01 3.275781250000E+02-4.157902561103E-01-7.082080711374E-09
+ -2.339383158984E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00-1.040000000000E-08-1.040000000000E-08
+ 3.858180000000E+05 1.000000000000E+00
+C32 2019 12 12 11 00 00-8.566775359213E-04 2.992273095970E-12 0.000000000000E+00
+ 1.000000000000E+00 2.875000000000E+01 4.228033257413E-09 2.867304095205E+00
+ 1.456588506699E-06 1.842749770731E-04 6.427057087421E-06 5.282614164352E+03
+ 3.852000000000E+05 1.862645149231E-09 1.148173467908E+00 6.705522537231E-08
+ 9.608588315814E-01 2.297500000000E+02-1.050918258529E+00-6.857785654299E-09
+ 5.228789228631E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00-9.900000000000E-09-9.900000000000E-09
+ 3.857580000000E+05 1.000000000000E+00
+C37 2019 12 12 11 00 00-8.880557725206E-04-5.954881032721E-11 0.000000000000E+00
+ 1.000000000000E+00 1.086562500000E+02 3.521932416908E-09 8.066563687499E-01
+ 5.388632416725E-06 7.037379546091E-04 1.127412542701E-05 5.282630052567E+03
+ 3.852000000000E+05-2.142041921616E-08-3.020306267399E+00 1.536682248116E-08
+ 9.560827215058E-01 1.306718750000E+02-1.080448983277E+00-6.580988410297E-09
+ -2.328668426958E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00-1.390000000000E-08-1.390000000000E-08
+ 3.858180000000E+05 1.000000000000E+00
+C20 2019 12 12 11 00 00-4.067538538948E-04-5.008171655163E-11 0.000000000000E+00
+ 1.000000000000E+00 2.850000000000E+01 4.229461888350E-09 1.463145840983E+00
+ 1.523178070784E-06 5.460308166221E-04 6.177462637424E-06 5.282614208221E+03
+ 3.852000000000E+05 3.445893526077E-08 1.153432718382E+00-1.210719347000E-08
+ 9.622213291121E-01 2.341406250000E+02-4.480160490039E-01-6.852785446020E-09
+ 4.753769442150E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00 2.090000000000E-08 2.090000000000E-08
+ 3.875880000000E+05 1.000000000000E+00
diff --git a/src/examples/sample_data.obs b/src/examples/sample_data.obs
new file mode 100644
index 0000000000000000000000000000000000000000..d773d3b4d57556afcd38b64ef4167981a00de992
--- /dev/null
+++ b/src/examples/sample_data.obs
@@ -0,0 +1,58 @@
+ 3.04 OBSERVATION DATA M RINEX VERSION / TYPE
+sbf2rin-13.4.3 20200205 113408 UTC PGM / RUN BY / DATE
+SEPT MARKER NAME
+Unknown MARKER NUMBER
+Unknown Unknown OBSERVER / AGENCY
+3021420 SEPT ASTERX-M2 4.4.0 REC # / TYPE / VERS
+Unknown Unknown ANT # / TYPE
+ 4789398.3686 176958.8129 4194502.0999 APPROX POSITION XYZ
+ 0.0000 0.0000 0.0000 ANTENNA: DELTA H/E/N
+G 7 X1 C1C L1C C2W L2W C2L L2L SYS / # / OBS TYPES
+E 7 X1 C1C L1C C5Q L5Q C7Q L7Q SYS / # / OBS TYPES
+S 3 X1 C1C L1C SYS / # / OBS TYPES
+R 5 X1 C1C L1C C2C L2C SYS / # / OBS TYPES
+C 5 X1 C2I L2I C7I L7I SYS / # / OBS TYPES
+SEPTENTRIO RECEIVERS OUTPUT ALIGNED CARRIER PHASES. COMMENT
+NO FURTHER PHASE SHIFT APPLIED IN THE RINEX ENCODER. COMMENT
+G L1C SYS / PHASE SHIFT
+G L2W SYS / PHASE SHIFT
+G L2L 0.00000 SYS / PHASE SHIFT
+E L1C 0.00000 SYS / PHASE SHIFT
+E L5Q 0.00000 SYS / PHASE SHIFT
+E L7Q 0.00000 SYS / PHASE SHIFT
+S L1C SYS / PHASE SHIFT
+R L1C SYS / PHASE SHIFT
+R L2C SYS / PHASE SHIFT
+C L2I SYS / PHASE SHIFT
+C L7I SYS / PHASE SHIFT
+ 0.050 INTERVAL
+ 2019 12 12 11 37 42.0000000 GPS TIME OF FIRST OBS
+ 2019 12 12 11 45 12.9000000 GPS TIME OF LAST OBS
+ 36 # OF SATELLITES
+ C1C 0.000 C2C 0.000 GLONASS COD/PHS/BIS
+ 8 R03 5 R05 1 R13 -2 R14 -7 R15 0 R19 3 R20 2 R21 4 GLONASS SLOT / FRQ #
+ END OF HEADER
+> 2019 12 12 11 37 42.0000000 0 23
+C05 16.000 40138840.659 6 209013478.40606 40138832.256 6 161622441.87406
+C10 17.000 39633104.355 5 206379985.90205 39633100.852 6 159586077.69206
+C23 18.000 22865676.301 8 119067585.90908
+C32 19.000 22153454.599 8 115358921.19508
+C37 20.000 23690566.611 7 123363010.77907
+E01 11.000 26311074.676 7 138265636.53007 26311074.470 5 103250320.19005 26311070.977 7 105943790.57907
+E09 12.000 25723493.482 5
+E12 13.000 24803095.627 5 130341141.55605 24803092.768 4 97332661.71504 24803092.407 6 99871767.93506
+E19 14.000 24540665.652 6 128962068.93606 24540664.837 5 96302837.53105 24540661.694 6 98815075.63606
+E21 15.000 25609802.251 7 134580416.73507 25609802.381 7 100498367.38007 25609799.519 7 103120055.01507
+G02 1.000 22032430.127 5
+G06 2.000 22147435.410 6 116385570.45206 22147437.095 4 90690045.24704 22147437.422 7 90690046.16907
+G07 3.000 20902411.481 8 109842911.60108 20902406.002 6 85591857.15006 20902406.930 7 85591857.16507
+G09 4.000 21908403.525 6 115129430.37406 21908399.544 1 89711233.32401 21908399.320 5
+G23 5.000 24104714.314 6 126671129.49306 24104706.816 2 98704739.67602
+G30 6.000 21404145.095 8 112479542.93308 21404142.953 6 87646388.26706 21404143.916 7 87646383.27307
+R03 7.000 21945110.617 7 117474090.63807 21945109.720 6 91368580.68606
+R14 8.000 20214975.526 8 107757316.39008 20214976.579 7 83811176.41707
+R15 9.000 22699008.378 5 121296675.96005 22699009.712 5 94341922.34705
+R19 10.000 22853592.745 4 22853590.888 4
+S23 21.000 38309228.895 7 201316353.13407
+S25 22.000 37834172.957 6 198818604.39206
+S36 23.000 37630702.258 7 197750698.01307
diff --git a/src/examples/trace b/src/examples/trace
new file mode 100644
index 0000000000000000000000000000000000000000..362c7af58c323df3a6d40b9e316a4b74679ddb30
--- /dev/null
+++ b/src/examples/trace
@@ -0,0 +1,86 @@
+3 readrnxt: file=../src/examples/sample_data.nav rcv=1
+3 expath : path=../src/examples/sample_data.nav nmax=1024
+3 expath : file=../src/examples/sample_data.nav
+3 readrnxfile: file=../src/examples/sample_data.nav flag=0 index=1
+3 rtk_uncompress: file=../src/examples/sample_data.nav
+3 rtk_uncompress: stat=0
+3 readrnxfp: flag=0 index=1
+3 readrnxh:
+4 decode_navh:
+4 decode_navh:
+4 decode_navh:
+4 decode_navh:
+4 decode_navh:
+4 decode_navh:
+4 decode_navh:
+4 decode_navh:
+3 readrnxnav: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 decode_seph: ver=3.04 sat=49
+4 readrnxnavb: ver=3.04 sys=0
+4 decode_seph: ver=3.04 sat=38
+4 readrnxnavb: ver=3.04 sys=0
+4 decode_seph: ver=3.04 sat=36
+4 readrnxnavb: ver=3.04 sys=0
+4 decode_seph: ver=3.04 sat=49
+4 readrnxnavb: ver=3.04 sys=0
+4 decode_seph: ver=3.04 sat=36
+4 readrnxnavb: ver=3.04 sys=0
+4 decode_seph: ver=3.04 sat=36
+4 readrnxnavb: ver=3.04 sys=0
+4 decode_seph: ver=3.04 sat=49
+4 readrnxnavb: ver=3.04 sys=0
+4 decode_seph: ver=3.04 sat=49
+4 readrnxnavb: ver=3.04 sys=0
+4 decode_seph: ver=3.04 sat=36
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+4 readrnxnavb: ver=3.04 sys=0
+3 uniqnav: neph=0 ngeph=0 nseph=9
+3 uniqeph: n=0
+3 uniqgeph: ng=0
+3 uniqseph: ns=9
+4 uniqseph: ns=9
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
deleted file mode 100644
index 16f173b9ee18f2a690af3dbd67574c852d222b89..0000000000000000000000000000000000000000
--- a/src/gnss_utils.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-#include "../include/gnss_utils.h"
-
-using namespace GNSSUtils;
-
-/******************************* RECEIVER CLASS *******************************************/
-
-Receiver::Receiver()
-{
-
-}
-
-Receiver::~Receiver()
-{
-
-}
-
-
-/* - Observation - */
-
-void Receiver::clearObservations()
-{
- _obsVector.clear();
-}
-
-void Receiver::pushObservation(obsd_t obs)
-{
- _obsVector.push_back(obs);
-}
-
-std::vector<obsd_t> Receiver::getObservations()
-{
- return this->_obsVector;
-}
-
-
-/* - Navigation - */
-
-void Receiver::clearNavigation()
-{
- _navVector.clear();
-}
-
-void Receiver::pushNavigation(nav_t nav)
-{
- _navVector.push_back(nav);
-}
-
-std::vector<nav_t> Receiver::getNavigation()
-{
- return this->_navVector;
-}
-
-
-
-
-
-
-
-int Receiver::getSPP(double *azel, char *msg)
-{
- obsd_t *obs = &this->_obsVector[0];
- int n = this->_obsVector.size();
-
- return int pntpos(*obs, n, nav_t *nav, *opt, *sol, *azel, *ssat, *msg)
-}
-
-
-
-
-
-
diff --git a/src/navigation.cpp b/src/navigation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c9d86a92b95700462279c3558dcb4008d913e0df
--- /dev/null
+++ b/src/navigation.cpp
@@ -0,0 +1,459 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gnss_utils/navigation.h"
+
+using namespace GnssUtils;
+
+Navigation::Navigation()
+{
+ // array initialization
+ nav_.n = nav_.nmax = 0;
+ nav_.ng = nav_.ngmax = 0;
+ nav_.ns = nav_.nsmax = 0;
+ nav_.ne = nav_.nemax = 0;
+ nav_.nc = nav_.ncmax = 0;
+ nav_.na = nav_.namax = 0;
+ nav_.nt = nav_.ntmax = 0;
+ nav_.nf = nav_.nfmax = 0;
+ nav_.eph = NULL;
+ nav_.geph = NULL;
+ nav_.seph = NULL;
+ nav_.peph = NULL;
+ nav_.pclk = NULL;
+ nav_.alm = NULL;
+ nav_.tec = NULL;
+ nav_.fcb = NULL;
+ clearNavigation();
+}
+
+Navigation::Navigation(const Navigation& nav)
+{
+ // array initialization
+ nav_.n = nav_.nmax = 0;
+ nav_.ng = nav_.ngmax = 0;
+ nav_.ns = nav_.nsmax = 0;
+ nav_.ne = nav_.nemax = 0;
+ nav_.nc = nav_.ncmax = 0;
+ nav_.na = nav_.namax = 0;
+ nav_.nt = nav_.ntmax = 0;
+ nav_.nf = nav_.nfmax = 0;
+ nav_.eph = NULL;
+ nav_.geph = NULL;
+ nav_.seph = NULL;
+ nav_.peph = NULL;
+ nav_.pclk = NULL;
+ nav_.alm = NULL;
+ nav_.tec = NULL;
+ nav_.fcb = NULL;
+ clearNavigation();
+ setNavigation(nav.getNavigation());
+}
+
+Navigation::~Navigation()
+{
+ freeNavigationArrays();
+}
+
+void Navigation::setNavigation(nav_t _nav)
+{
+ freeNavigationArrays();
+
+ copyAllArrays(_nav);
+ // int n,nmax; /* number of broadcast ephemeris */
+ // int ng,ngmax; /* number of glonass ephemeris */
+ // int ns,nsmax; /* number of sbas ephemeris */
+ // int ne,nemax; /* number of precise ephemeris */
+ // int nc,ncmax; /* number of precise clock */
+ // int na,namax; /* number of almanac data */
+ // int nt,ntmax; /* number of tec grid data */
+ // int nf,nfmax; /* number of satellite fcb data */
+ // eph_t *eph; /* GPS/QZS/GAL ephemeris */
+ // geph_t *geph; /* GLONASS ephemeris */
+ // seph_t *seph; /* SBAS ephemeris */
+ // peph_t *peph; /* precise ephemeris */
+ // pclk_t *pclk; /* precise clock */
+ // alm_t *alm; /* almanac data */
+ // tec_t *tec; /* tec grid data */
+ // fcbd_t *fcb; /* satellite fcb data */
+ // double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */
+
+ copyIonUtc(_nav);
+ // double utc_gps[4]; /* GPS delta-UTC parameters {A0,A1,T,W} */
+ // double utc_glo[4]; /* GLONASS UTC GPS time parameters */
+ // double utc_gal[4]; /* Galileo UTC GPS time parameters */
+ // double utc_qzs[4]; /* QZS UTC GPS time parameters */
+ // double utc_cmp[4]; /* BeiDou UTC parameters */
+ // double utc_irn[4]; /* IRNSS UTC parameters */
+ // double utc_sbs[4]; /* SBAS UTC parameters */
+ // double ion_gps[8]; /* GPS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+ // double ion_gal[4]; /* Galileo iono model parameters {ai0,ai1,ai2,0} */
+ // double ion_qzs[8]; /* QZSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+ // double ion_cmp[8]; /* BeiDou iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+ // double ion_irn[8]; /* IRNSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+ // int leaps; /* leap seconds (s) */
+
+ copySbasCorrections(_nav);
+ // sbssat_t sbssat; /* SBAS satellite corrections */
+ // sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */
+ // pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */
+
+ copyBias(_nav);
+ // double cbias[MAXSAT][3]; /* satellite dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */
+ // double rbias[MAXRCV][2][3]; /* receiver dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */
+ // double wlbias[MAXSAT]; /* wide-lane bias (cycle) */
+ // double glo_cpbias[4]; /* glonass code-phase bias {1C,1P,2C,2P} (m) */
+ // char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */
+
+
+ // ********** other not copied nav_t content: ***********
+ // erp_t erp; /* earth rotation parameters */
+ // dgps_t dgps[MAXSAT]; /* DGPS corrections */
+ // ssr_t ssr[MAXSAT]; /* SSR corrections */
+ // lexeph_t lexeph[MAXSAT]; /* LEX ephemeris */
+ // lexion_t lexion; /* LEX ionosphere correction */
+ // pppcorr_t pppcorr; /* ppp corrections */
+}
+
+void Navigation::clearNavigation()
+{
+ freeNavigationArrays();
+
+ memset(nav_.utc_gps, 0, sizeof(nav_.utc_gps));
+ memset(nav_.utc_glo, 0, sizeof(nav_.utc_glo));
+ memset(nav_.utc_gal, 0, sizeof(nav_.utc_gal));
+ memset(nav_.utc_qzs, 0, sizeof(nav_.utc_qzs));
+ memset(nav_.utc_cmp, 0, sizeof(nav_.utc_cmp));
+ memset(nav_.utc_irn, 0, sizeof(nav_.utc_irn));
+ memset(nav_.utc_sbs, 0, sizeof(nav_.utc_sbs));
+
+ memset(nav_.ion_gps, 0, sizeof(nav_.ion_gps));
+ memset(nav_.ion_gal, 0, sizeof(nav_.ion_gal));
+ memset(nav_.ion_qzs, 0, sizeof(nav_.ion_qzs));
+ memset(nav_.ion_cmp, 0, sizeof(nav_.ion_cmp));
+ memset(nav_.ion_irn, 0, sizeof(nav_.ion_irn));
+
+ memset(nav_.cbias, 0, sizeof(nav_.cbias));
+ memset(nav_.rbias, 0, sizeof(nav_.rbias));
+ memset(nav_.wlbias, 0, sizeof(nav_.wlbias));
+ memset(nav_.glo_cpbias, 0, sizeof(nav_.glo_cpbias));
+ memset(nav_.glo_fcn, 0, sizeof(nav_.glo_fcn));
+
+ nav_.leaps = 0;
+ sbssat_t sbssat_zero = { 0 };
+ for (auto&& sat : sbssat_zero.sat)
+ sat.lcorr.iode = -1;
+ nav_.sbssat = sbssat_zero;
+}
+
+void Navigation::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt)
+{
+ auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, NULL, &nav_, NULL);
+ if (stat == 1)
+ {
+ std::cout << "Navigation file loaded." << std::endl;
+ std::cout << "GPS satellites in navigation file: " << nav_.n << std::endl;
+ std::cout << "GLONASS satellites in navigation file: " << nav_.ng << std::endl;
+ std::cout << "SBAS satellites in navigation file: " << nav_.ns << std::endl;
+ std::cout << "Almanac satellites in navigation file: " << nav_.na << std::endl;
+ uniqueNavigation();
+ }
+ else
+ std::cout << "Navigation: couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error")
+ << std::endl;
+}
+
+void Navigation::addSbasMessage(const sbsmsg_t& sbas_msg)
+{
+ // init seph with array of size NSATSBS*2
+ if (nav_.seph == NULL)
+ {
+ //std::cout << "Navigation::addSbasMessage: initializing sbas ephemerides array...\n";
+ if (!(nav_.seph =(seph_t *)malloc(sizeof(seph_t)*NSATSBS*2)))
+ {
+ std::cout << "Navigation::addSbasMessage: could not malloc sbas ephemerides when initializing sbas ephemerides array!\n";
+ return;
+ }
+ nav_.ns=NSATSBS*2;
+ nav_.nsmax=NSATSBS*2;
+ for (auto i=0; i<NSATSBS*2; i++)
+ nav_.seph[i]=seph_t{0};
+ }
+ sbsupdatecorr(&sbas_msg, &nav_);
+}
+
+void Navigation::copyAllArrays(const nav_t& nav)
+{
+ // GPS/QZS/GAL ephemeris
+ copyArray<eph_t>(nav.eph, nav.n, nav_.eph, nav_.n, nav_.nmax);
+ // GLONASS ephemeris
+ copyArray<geph_t>(nav.geph, nav.ng, nav_.geph, nav_.ng, nav_.ngmax);
+ // SBAS ephemeris
+ copyArray<seph_t>(nav.seph, nav.ns, nav_.seph, nav_.ns, nav_.nsmax);
+ // precise ephemeris
+ copyArray<peph_t>(nav.peph, nav.ne, nav_.peph, nav_.ne, nav_.nemax);
+ // precise clock
+ copyArray<pclk_t>(nav.pclk, nav.nc, nav_.pclk, nav_.nc, nav_.ncmax);
+ // almanac data
+ copyArray<alm_t>(nav.alm, nav.na, nav_.alm, nav_.na, nav_.namax);
+ // tec grid data
+ copyArray<tec_t>(nav.tec, nav.nt, nav_.tec, nav_.nt, nav_.ntmax);
+ // satellite fcb data
+ copyArray<fcbd_t>(nav.fcb, nav.nf, nav_.fcb, nav_.nf, nav_.nfmax);
+
+ uniqueNavigation();
+}
+
+void Navigation::copyEphemeris(const nav_t& nav)
+{
+ // Not SBAS ephemeris!
+
+ // copy
+ copyArray<eph_t>(nav.eph, nav.n, nav_.eph, nav_.n, nav_.nmax);
+ copyArray<geph_t>(nav.geph, nav.ng, nav_.geph, nav_.ng, nav_.ngmax);
+
+ // unique
+ uniqeph (&nav_);
+ uniqgeph(&nav_);
+
+ // wavelengths
+ updatenav(&nav_);
+}
+
+void Navigation::copyAlmanac(const nav_t& nav)
+{
+ copyArray<alm_t>(nav.alm, nav.na, nav_.alm, nav_.na, nav_.namax);
+}
+
+void Navigation::copyIonUtc(const nav_t& nav)
+{
+ std::copy(nav.utc_gps, nav.utc_gps + ARRAY_SIZE(nav.utc_gps), nav_.utc_gps);
+ std::copy(nav.utc_gal, nav.utc_gal + ARRAY_SIZE(nav.utc_gal), nav_.utc_gal);
+ std::copy(nav.utc_qzs, nav.utc_qzs + ARRAY_SIZE(nav.utc_qzs), nav_.utc_qzs);
+ std::copy(nav.utc_glo, nav.utc_glo + ARRAY_SIZE(nav.utc_glo), nav_.utc_glo);
+ std::copy(nav.utc_cmp, nav.utc_cmp + ARRAY_SIZE(nav.utc_cmp), nav_.utc_cmp);
+ std::copy(nav.utc_irn, nav.utc_irn + ARRAY_SIZE(nav.utc_irn), nav_.utc_irn);
+ std::copy(nav.utc_sbs, nav.utc_sbs + ARRAY_SIZE(nav.utc_sbs), nav_.utc_sbs);
+
+ std::copy(nav.ion_gps, nav.ion_gps + ARRAY_SIZE(nav.ion_gps), nav_.ion_gps);
+ std::copy(nav.ion_gal, nav.ion_gal + ARRAY_SIZE(nav.ion_gal), nav_.ion_gal);
+ std::copy(nav.ion_qzs, nav.ion_qzs + ARRAY_SIZE(nav.ion_qzs), nav_.ion_qzs);
+ std::copy(nav.ion_cmp, nav.ion_cmp + ARRAY_SIZE(nav.ion_cmp), nav_.ion_cmp);
+ std::copy(nav.ion_irn, nav.ion_irn + ARRAY_SIZE(nav.ion_irn), nav_.ion_irn);
+
+ nav_.leaps = nav.leaps;
+}
+
+void Navigation::copySbasCorrections(const nav_t& nav)
+{
+ nav_.sbssat.iodp = nav.sbssat.iodp;
+ nav_.sbssat.nsat = nav.sbssat.nsat;
+ assert(nav.sbssat.nsat < MAXSAT);
+ nav_.sbssat.tlat = nav.sbssat.tlat;
+ for (int i = 0; i < MAXSAT; i++)
+ {
+ nav_.sbssat.sat[i].sat = nav.sbssat.sat[i].sat;
+ nav_.sbssat.sat[i].fcorr = nav.sbssat.sat[i].fcorr;
+ nav_.sbssat.sat[i].lcorr.daf0 = nav.sbssat.sat[i].lcorr.daf0;
+ nav_.sbssat.sat[i].lcorr.daf1 = nav.sbssat.sat[i].lcorr.daf1;
+ nav_.sbssat.sat[i].lcorr.iode = nav.sbssat.sat[i].lcorr.iode;
+ nav_.sbssat.sat[i].lcorr.t0 = nav.sbssat.sat[i].lcorr.t0;
+ std::copy(nav.sbssat.sat[i].lcorr.dpos, nav.sbssat.sat[i].lcorr.dpos + 3, nav_.sbssat.sat[i].lcorr.dpos);
+ std::copy(nav.sbssat.sat[i].lcorr.dvel, nav.sbssat.sat[i].lcorr.dvel + 3, nav_.sbssat.sat[i].lcorr.dvel);
+ }
+ for (int i = 0; i < MAXBAND + 1; i++)
+ {
+ nav_.sbsion[i].iodi = nav.sbsion[i].iodi;
+ nav_.sbsion[i].nigp = nav.sbsion[i].nigp;
+ assert(nav.sbsion[i].nigp <= MAXNIGP);
+ std::copy(nav.sbsion[i].igp, nav.sbsion[i].igp + MAXNIGP, nav_.sbsion[i].igp);
+ }
+ for (int i = 0; i < MAXSAT; i++)
+ {
+ std::copy(nav.pcvs[i].code, nav.pcvs[i].code + ARRAY_SIZE(nav.pcvs[i].code), nav_.pcvs[i].code);
+ std::copy(&nav.pcvs[i].off[0][0], &nav.pcvs[i].off[0][0] + ARRAY2D_SIZE(nav.pcvs[i].off), &nav_.pcvs[i].off[0][0]);
+ nav_.pcvs[i].sat = nav.pcvs[i].sat;
+ nav_.pcvs[i].te = nav.pcvs[i].te;
+ nav_.pcvs[i].ts = nav.pcvs[i].ts;
+ // std::cout << "copySbasCorrections : " << nav.pcvs[i].te.time << " + " << nav.pcvs[i].te.sec << "\n";
+ // std::cout << "copySbasCorrections : " << nav_.pcvs[i].te.time << " + " << nav_.pcvs[i].te.sec << "\n";
+ std::copy(nav.pcvs[i].type, nav.pcvs[i].type + ARRAY_SIZE(nav.pcvs[i].type), nav_.pcvs[i].type);
+ std::copy(&nav.pcvs[i].var[0][0], &nav.pcvs[i].var[0][0] + ARRAY2D_SIZE(nav.pcvs[i].var), &nav_.pcvs[i].var[0][0]);
+ }
+}
+
+void Navigation::copyBias(const nav_t& nav)
+{
+ std::copy(&nav.cbias[0][0], &nav.cbias[0][0] + ARRAY2D_SIZE(nav.cbias), &nav_.cbias[0][0]);
+ std::copy(&nav.rbias[0][0][0], &nav.rbias[0][0][0] + ARRAY3D_SIZE(nav.rbias), &nav_.rbias[0][0][0]);
+
+ std::copy(nav.wlbias, nav.wlbias + ARRAY_SIZE(nav.wlbias), nav_.wlbias);
+ std::copy(nav.glo_cpbias, nav.glo_cpbias + ARRAY_SIZE(nav.glo_cpbias), nav_.glo_cpbias);
+ std::copy(nav.glo_fcn, nav.glo_fcn + ARRAY_SIZE(nav.glo_fcn), nav_.glo_fcn);
+}
+
+void Navigation::freeNavigationArrays()
+{
+ // RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
+ freeEphemeris();
+ // std::cout << "freeing glonass ephemeris...\n";
+ freeGlonassEphemeris();
+ // std::cout << "freeing sbas ephemeris...\n";
+ freeSbasEphemeris();
+ // std::cout << "freeing precise ephemeris...\n";
+ freePreciseEphemeris();
+ // std::cout << "freeing precise clock...\n";
+ freePreciseClock();
+ // std::cout << "freeing almanac...\n";
+ freeAlmanac();
+ // std::cout << "freeing tec...\n";
+ freeTecData();
+ // std::cout << "freeing fcb...\n";
+ freeFcbData();
+}
+
+void Navigation::print()
+{
+ std::cout << "n: " << nav_.n << "\t(nmax: " << nav_.nmax << ")\n";
+ std::cout << "ng: " << nav_.ng << "\t(nfmax: " << nav_.nfmax << ")\n";
+ std::cout << "ns: " << nav_.ns << "\t(nsmax: " << nav_.nsmax << ")\n";
+ std::cout << "ne: " << nav_.ne << "\t(nemax: " << nav_.nemax << ")\n";
+ std::cout << "nc: " << nav_.nc << "\t(ncmax: " << nav_.ncmax << ")\n";
+ std::cout << "na: " << nav_.na << "\t(namax: " << nav_.namax << ")\n";
+ std::cout << "nt: " << nav_.nt << "\t(ntmax: " << nav_.ntmax << ")\n";
+ std::cout << "nf: " << nav_.nf << "\t(nfmax: " << nav_.nfmax << ")\n";
+
+ // TODO
+ // eph_t *eph; /* GPS/QZS/GAL ephemeris */
+ // geph_t *geph; /* GLONASS ephemeris */
+ // seph_t *seph; /* SBAS ephemeris */
+ // peph_t *peph; /* precise ephemeris */
+ // pclk_t *pclk; /* precise clock */
+ // alm_t *alm; /* almanac data */
+ // tec_t *tec; /* tec grid data */
+ // fcbd_t *fcb; /* satellite fcb data */
+ // double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */
+
+ printArray<double, ARRAY_SIZE(nav_.utc_gps)>("utc_gps: ", nav_.utc_gps);
+ printArray<double, ARRAY_SIZE(nav_.utc_glo)>("utc_glo: ", nav_.utc_glo);
+ printArray<double, ARRAY_SIZE(nav_.utc_gal)>("utc_gal: ", nav_.utc_gal);
+ printArray<double, ARRAY_SIZE(nav_.utc_qzs)>("utc_qzs: ", nav_.utc_qzs);
+ printArray<double, ARRAY_SIZE(nav_.utc_cmp)>("utc_cmp: ", nav_.utc_cmp);
+ printArray<double, ARRAY_SIZE(nav_.utc_irn)>("utc_irn: ", nav_.utc_irn);
+ printArray<double, ARRAY_SIZE(nav_.utc_sbs)>("utc_sbs: ", nav_.utc_sbs);
+ printArray<double, ARRAY_SIZE(nav_.ion_gps)>("ion_gps: ", nav_.ion_gps);
+ printArray<double, ARRAY_SIZE(nav_.ion_gal)>("ion_gal: ", nav_.ion_gal);
+ printArray<double, ARRAY_SIZE(nav_.ion_qzs)>("ion_qzs: ", nav_.ion_qzs);
+ printArray<double, ARRAY_SIZE(nav_.ion_cmp)>("ion_cmp: ", nav_.ion_cmp);
+ printArray<double, ARRAY_SIZE(nav_.ion_irn)>("ion_irn: ", nav_.ion_irn);
+
+ std::cout << "leaps: " << nav_.leaps << "\n";
+
+ std::cout << "lam: \n";
+ for (int i = 0; i < ARRAY2D_NROWS(nav_.lam); i++)
+ printArray<double, ARRAY2D_NCOLS(nav_.lam)>("\t", nav_.lam[i]);
+
+ std::cout << "cbias: \n";
+ for (int i = 0; i < ARRAY2D_NROWS(nav_.cbias); i++)
+ printArray<double, ARRAY2D_NCOLS(nav_.cbias)>("\t", nav_.cbias[i]);
+
+ printArray<double, ARRAY_SIZE(nav_.wlbias)> ("wlbias: ", nav_.wlbias);
+ printArray<double, ARRAY_SIZE(nav_.glo_cpbias)>("glo_cpbias: ", nav_.glo_cpbias);
+ printArray<char, ARRAY_SIZE(nav_.glo_fcn)> ("glo_fcn: ", nav_.glo_fcn);
+
+ std::cout << "pcvs: \n";
+ for (int i = 0; i < ARRAY_SIZE(nav_.pcvs); i++)
+ {
+ std::cout << "\tsat: " << nav_.pcvs[i].sat << "\n";
+ printArray<char, ARRAY_SIZE(nav_.pcvs[i].type)>("\t\ttype: ", nav_.pcvs[i].type);
+ printArray<char, ARRAY_SIZE(nav_.pcvs[i].code)>("\t\tcode: ", nav_.pcvs[i].code);
+
+ std::cout << "\t\tte: " << nav_.pcvs[i].te.time << " + "<< nav_.pcvs[i].te.sec << "\n";
+ std::cout << "\t\tts: " << nav_.pcvs[i].ts.time << " + "<< nav_.pcvs[i].ts.sec << "\n";
+ std::cout << "\t\toff: \n";
+ for (int j = 0; j < ARRAY2D_NROWS(nav_.pcvs[i].off); j++)
+ printArray<double, ARRAY2D_NCOLS(nav_.pcvs[i].off)>("\t\t\t", nav_.pcvs[i].off[j]);
+ std::cout << "\t\tvar: \n";
+ for (int j = 0; j < ARRAY2D_NROWS(nav_.pcvs[i].var); j++)
+ printArray<double, ARRAY2D_NCOLS(nav_.pcvs[i].var)>("\t\t\t", nav_.pcvs[i].var[j]);
+ }
+
+ if (nav_.sbssat.nsat == 0)
+ std::cout << "sbssat: empty\n";
+ else
+ {
+ std::cout << "sbssat: \n";
+ std::cout << "\tiodp: " << nav_.sbssat.iodp << "\n";
+ std::cout << "\tnsat: " << nav_.sbssat.nsat << "\n";
+ std::cout << "\ttlat: " << nav_.sbssat.tlat << "\n";
+ for (int i = 0; i < nav_.sbssat.nsat; i++)
+ {
+ std::cout << "\tsat: " << i << "\n";
+ std::cout << "\t\tsat " << nav_.sbssat.sat[i].sat << "\n";
+
+ std::cout << "\t\tfcorr.t0 " << nav_.sbssat.sat[i].fcorr.t0.time << " + "<< nav_.sbssat.sat[i].fcorr.t0.sec << "\n";
+ std::cout << "\t\tfcorr.prc " << nav_.sbssat.sat[i].fcorr.prc << "\n";
+ std::cout << "\t\tfcorr.rrc " << nav_.sbssat.sat[i].fcorr.rrc << "\n";
+ std::cout << "\t\tfcorr.dt " << nav_.sbssat.sat[i].fcorr.dt << "\n";
+ std::cout << "\t\tfcorr.iodf " << nav_.sbssat.sat[i].fcorr.iodf << "\n";
+ std::cout << "\t\tfcorr.udre " << nav_.sbssat.sat[i].fcorr.udre << "\n";
+ std::cout << "\t\tfcorr.ai " << nav_.sbssat.sat[i].fcorr.ai << "\n";
+
+ std::cout << "\t\tlcorr.daf0 " << nav_.sbssat.sat[i].lcorr.daf0 << "\n";
+ std::cout << "\t\tlcorr.daf1 " << nav_.sbssat.sat[i].lcorr.daf1 << "\n";
+ std::cout << "\t\tlcorr.iode " << nav_.sbssat.sat[i].lcorr.iode << "\n";
+ std::cout << "\t\tlcorr.t0 " << nav_.sbssat.sat[i].lcorr.t0.time << " + "<< nav_.sbssat.sat[i].lcorr.t0.sec << "\n";
+ printArray<double, ARRAY_SIZE(nav_.sbssat.sat[i].lcorr.dpos)>("\t\tlcorr.dpos: ", nav_.sbssat.sat[i].lcorr.dpos);
+ printArray<double, ARRAY_SIZE(nav_.sbssat.sat[i].lcorr.dvel)>("\t\tlcorr.dvel: ", nav_.sbssat.sat[i].lcorr.dvel);
+ }
+ }
+ for (int i = 0; i < MAXBAND + 1; i++)
+ {
+ if (nav_.sbsion[i].nigp == 0)
+ std::cout << "sbsion: " << i << " empty\n";
+ else
+ {
+ std::cout << "sbsion: " << i << "\n";
+ std::cout << "\tiodi: " << nav_.sbsion[i].iodi << "\n";
+ std::cout << "\tnigp: " << nav_.sbsion[i].nigp << "\n";
+ for (int j = 0; j < nav_.sbsion[i].nigp; j++)
+ {
+ std::cout << "\tigp: " << j << "\n";
+ std::cout << "\t\tt0: " << nav_.sbsion[i].igp[j].t0.time << " + "<< nav_.sbsion[i].igp[j].t0.sec << "\n";
+ std::cout << "\t\tlat: " << nav_.sbsion[i].igp[j].lat << "\n";
+ std::cout << "\t\tlon: " << nav_.sbsion[i].igp[j].lon << "\n";
+ std::cout << "\t\tgive: " << nav_.sbsion[i].igp[j].give << "\n";
+ std::cout << "\t\tdelay: " << nav_.sbsion[i].igp[j].delay << "\n";
+ }
+ }
+ }
+
+ // TODO:
+ // double rbias[MAXRCV][2][3]; /* receiver dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */
+
+ // ********** other not copied nav_t content: ***********
+ // erp_t erp; /* earth rotation parameters */
+ // dgps_t dgps[MAXSAT]; /* DGPS corrections */
+ // ssr_t ssr[MAXSAT]; /* SSR corrections */
+ // lexeph_t lexeph[MAXSAT]; /* LEX ephemeris */
+ // lexion_t lexion; /* LEX ionosphere correction */
+ // pppcorr_t pppcorr; /* ppp corrections */
+}
diff --git a/src/observations.cpp b/src/observations.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4df0dbbd1ffafa49cdcd5a5868854426583b60c1
--- /dev/null
+++ b/src/observations.cpp
@@ -0,0 +1,536 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+#include "gnss_utils/utils/satellite.h"
+
+using namespace GnssUtils;
+
+Observations::Observations()
+{
+}
+
+Observations::Observations(const Observations& obs)
+ : sat_2_idx_(obs.sat_2_idx_), idx_2_sat_(obs.idx_2_sat_), obs_(obs.obs_.size())
+{
+ // copy all elements
+ for (auto i = 0; i < obs.obs_.size(); i++)
+ {
+ obs_[i].time = obs.obs_[i].time;
+ obs_[i].eventime = obs.obs_[i].eventime;
+ obs_[i].timevalid = obs.obs_[i].timevalid;
+ obs_[i].sat = obs.obs_[i].sat;
+ obs_[i].rcv = obs.obs_[i].rcv;
+ std::copy(obs.obs_[i].SNR, obs.obs_[i].SNR + NFREQ + NEXOBS, obs_[i].SNR);
+ std::copy(obs.obs_[i].LLI, obs.obs_[i].LLI + NFREQ + NEXOBS, obs_[i].LLI);
+ std::copy(obs.obs_[i].code, obs.obs_[i].code + NFREQ + NEXOBS, obs_[i].code);
+ std::copy(obs.obs_[i].qualL, obs.obs_[i].qualL + NFREQ + NEXOBS, obs_[i].qualL);
+ std::copy(obs.obs_[i].qualP, obs.obs_[i].qualP + NFREQ + NEXOBS, obs_[i].qualP);
+ obs_[i].freq = obs.obs_[i].freq;
+ std::copy(obs.obs_[i].L, obs.obs_[i].L + NFREQ + NEXOBS, obs_[i].L);
+ std::copy(obs.obs_[i].P, obs.obs_[i].P + NFREQ + NEXOBS, obs_[i].P);
+ std::copy(obs.obs_[i].D, obs.obs_[i].D + NFREQ + NEXOBS, obs_[i].D);
+ }
+}
+
+Observations::~Observations()
+{
+}
+
+void Observations::clearObservations()
+{
+ this->obs_.clear();
+ this->idx_2_sat_.clear();
+ this->sat_2_idx_.clear();
+}
+
+void Observations::addObservation(const obsd_t& obs)
+{
+ assert(sat_2_idx_.size() == idx_2_sat_.size());
+ assert(sat_2_idx_.size() == obs_.size());
+
+ // copy obsd_t object
+ obsd_t copy_obs;
+ copy_obs.time = obs.time;
+ copy_obs.eventime = obs.eventime;
+ copy_obs.timevalid = obs.timevalid;
+ copy_obs.sat = obs.sat;
+ copy_obs.rcv = obs.rcv;
+ std::copy(obs.SNR, obs.SNR + NFREQ + NEXOBS, copy_obs.SNR);
+ std::copy(obs.LLI, obs.LLI + NFREQ + NEXOBS, copy_obs.LLI);
+ std::copy(obs.code, obs.code + NFREQ + NEXOBS, copy_obs.code);
+ std::copy(obs.qualL, obs.qualL + NFREQ + NEXOBS, copy_obs.qualL);
+ std::copy(obs.qualP, obs.qualP + NFREQ + NEXOBS, copy_obs.qualP);
+ copy_obs.freq = obs.freq;
+ std::copy(obs.L, obs.L + NFREQ + NEXOBS, copy_obs.L);
+ std::copy(obs.P, obs.P + NFREQ + NEXOBS, copy_obs.P);
+ std::copy(obs.D, obs.D + NFREQ + NEXOBS, copy_obs.D);
+
+ assert(!hasSatellite(copy_obs.sat) && "adding an observation of a satellite already stored!");
+ this->obs_.push_back(copy_obs);
+ idx_2_sat_.push_back(copy_obs.sat);
+ sat_2_idx_[copy_obs.sat] = obs_.size() - 1;
+
+ // std::cout << "added observation of sat " << (int)obs.sat << " stored in idx " << sat_2_idx_[obs.sat] << std::endl;
+
+ assert(sat_2_idx_.size() == idx_2_sat_.size());
+ assert(sat_2_idx_.size() == obs_.size());
+}
+
+void Observations::removeObservationByIdx(const int& _idx)
+{
+ // std::cout << "removing observation of idx " << _idx << std::endl;
+ assert(sat_2_idx_.size() == idx_2_sat_.size());
+ assert(sat_2_idx_.size() == obs_.size());
+ assert(obs_.size() > _idx);
+ assert(idx_2_sat_.size() > _idx);
+ assert(sat_2_idx_.count(idx_2_sat_.at(_idx)) != 0);
+
+ // remove obs from sat_2_idx map
+ sat_2_idx_.erase(idx_2_sat_.at(_idx));
+ // decrease the idx of the observations after the removed one
+ for (auto&& sat_idx : sat_2_idx_)
+ if (sat_idx.second > _idx)
+ sat_idx.second--;
+ // remove obs from obs and idx_2_sat vectors
+ obs_.erase(obs_.begin() + _idx);
+ idx_2_sat_.erase(idx_2_sat_.begin() + _idx);
+
+ assert(sat_2_idx_.size() == idx_2_sat_.size());
+ assert(sat_2_idx_.size() == obs_.size());
+}
+
+void Observations::removeObservationBySat(const int& _sat)
+{
+ // std::cout << "removing observation of sat " << _sat << std::endl;
+ assert(sat_2_idx_.size() == idx_2_sat_.size());
+ assert(sat_2_idx_.size() == obs_.size());
+ assert(sat_2_idx_.count(_sat) != 0);
+ assert(hasSatellite(_sat));
+ assert(obs_.size() > sat_2_idx_.at(_sat));
+ assert(idx_2_sat_.size() > sat_2_idx_.at(_sat));
+
+ int idx = sat_2_idx_.at(_sat);
+ // remove obs from sat_2_idx map
+ sat_2_idx_.erase(_sat);
+ // decrease the idx of the observations after the removed one
+ for (auto&& sat_idx : sat_2_idx_)
+ if (sat_idx.second > idx)
+ sat_idx.second--;
+ // remove obs from obs and idx_2_sat vectors
+ obs_.erase(obs_.begin() + idx);
+ idx_2_sat_.erase(idx_2_sat_.begin() + idx);
+
+ assert(sat_2_idx_.size() == idx_2_sat_.size());
+ assert(sat_2_idx_.size() == obs_.size());
+}
+
+void Observations::print(obsd_t& _obs)
+{
+ std::string msg = "Observation of satellite #" + std::to_string(_obs.sat);
+ GnssUtils::print(msg);
+ std::cout << "\tTime [s]: " << _obs.time.time << " + " << _obs.time.sec << "\n";
+ std::cout << "\tTime valid: " << _obs.timevalid << std::endl;
+ printArray<unsigned char, ARRAY_SIZE(_obs.SNR)>("\tSNR: ", _obs.SNR);
+ printArray<unsigned char, ARRAY_SIZE(_obs.LLI)>("\tLLI: ", _obs.LLI);
+ printArray<unsigned char, ARRAY_SIZE(_obs.code)>("\tcode: ", _obs.code);
+ printArray<unsigned char, ARRAY_SIZE(_obs.qualL)>("\tqualL: ", _obs.qualL);
+ printArray<unsigned char, ARRAY_SIZE(_obs.qualP)>("\tqualP: ", _obs.qualP);
+ printf("\tFreq. channel: %uc \n", (int)_obs.freq);
+ printArray<double, ARRAY_SIZE(_obs.L)>("\tL: ", _obs.L);
+ printArray<double, ARRAY_SIZE(_obs.P)>("\tP: ", _obs.P);
+ printArray<float, ARRAY_SIZE(_obs.D)>("\tD: ", _obs.D);
+}
+
+void Observations::printBySat(const int& _sat_number)
+{
+ print(getObservationBySat(_sat_number));
+}
+
+void Observations::printByIdx(const int& _idx)
+{
+ print(getObservationByIdx(_idx));
+}
+
+void Observations::print()
+{
+ for (auto obs : obs_)
+ {
+ print(obs);
+ }
+}
+
+std::set<int> Observations::filterByEphemeris(const Satellites& sats)
+{
+ std::set<int> remove_sats;
+
+ for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+ {
+ auto&& obs_sat = getObservationByIdx(obs_i);
+ const int& sat_number = obs_sat.sat;
+
+ // inexistent satellite (satellite is not included in the discarded list)
+ if (sats.count(sat_number) == 0)
+ {
+ //std::cout << "Discarding sat " << sat_number << ": no information available" << std::endl;
+ remove_sats.insert(sat_number);
+ }
+ // bad satellite position (satellite is not included in the discarded list)
+ if (sats.at(sat_number).pos.isApprox(Eigen::Vector3d::Zero(), 1e-3) or
+ sats.at(sat_number).pos.norm() < 2.5e7)
+ {
+ //std::cout << "Discarding sat " << sat_number << ": wrong satellite position: " << sats.at(sat_number).pos.transpose() << std::endl;
+ remove_sats.insert(sat_number);
+ }
+ }
+
+ // remove sats
+ // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+ for (auto sat : remove_sats)
+ {
+ assert(hasSatellite(sat));
+ removeObservationBySat(sat);
+ }
+
+ return remove_sats;
+}
+
+std::set<int> Observations::filterBySatellites(const std::set<int>& discarded_sats)
+{
+ std::set<int> remove_sats;
+
+ for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+ {
+ auto&& obs_sat = getObservationByIdx(obs_i);
+ const int& sat_number = obs_sat.sat;
+
+ // discard sats
+ if (discarded_sats.count(sat_number) != 0)
+ remove_sats.insert(sat_number);
+ }
+
+ // remove sats
+ // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+ for (auto sat : remove_sats)
+ {
+ assert(hasSatellite(sat));
+ removeObservationBySat(sat);
+ }
+
+ return remove_sats;
+}
+
+std::set<int> Observations::filterByCode()
+{
+ std::set<int> remove_sats;
+
+ for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+ {
+ auto&& obs_sat = getObservationByIdx(obs_i);
+ const int& sat_number = obs_sat.sat;
+
+ // wrong data
+ if (std::abs(obs_sat.P[0]) < 1e-12)
+ {
+ //std::cout << "Discarding sat " << sat_number << ": wrong code data: " << obs_sat.P[0] << std::endl;
+ remove_sats.insert(sat_number);
+ }
+ }
+
+ // remove sats
+ // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+ for (auto sat : remove_sats)
+ {
+ assert(hasSatellite(sat));
+ removeObservationBySat(sat);
+ }
+
+ return remove_sats;
+}
+
+std::set<int> Observations::filterByCarrierPhase()
+{
+ std::set<int> remove_sats;
+
+ for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+ {
+ auto&& obs_sat = getObservationByIdx(obs_i);
+ const int& sat_number = obs_sat.sat;
+
+ // wrong data
+ if (std::abs(obs_sat.L[0]) < 1e-12)
+ {
+ //std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_sat.L[0] << std::endl;
+ remove_sats.insert(sat_number);
+ }
+ }
+
+ // remove sats
+ // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+ for (auto sat : remove_sats)
+ {
+ assert(hasSatellite(sat));
+ removeObservationBySat(sat);
+ }
+
+ return remove_sats;
+}
+
+std::set<int> Observations::filterByConstellations(const int& navsys)
+{
+ std::set<int> remove_sats;
+
+ for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+ {
+ auto&& obs_sat = getObservationByIdx(obs_i);
+ const int& sat_number = obs_sat.sat;
+
+ // constellation
+ int sys = satsys(obs_sat.sat, NULL);
+ if (!(sys & navsys))
+ {
+ //std::cout << "Discarding sat " << sat_number << ": not selected constellation " << sys << " - mask: " << navsys << std::endl;
+ remove_sats.insert(sat_number);
+ continue;
+ }
+ }
+
+ // remove sats
+ // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+ for (auto sat : remove_sats)
+ {
+ assert(hasSatellite(sat));
+ removeObservationBySat(sat);
+ }
+
+ return remove_sats;
+}
+
+std::set<int> Observations::filterByElevationSnr(const Azels& azels,
+ const snrmask_t& snrmask,
+ const double& elmin,
+ const bool &multi_freq)
+{
+ std::set<int> remove_sats;
+
+ for (auto&& sat_idx_pair : sat_2_idx_)
+ {
+ const int& sat_number = sat_idx_pair.first;
+ assert(azels.count(sat_number) && "azel missing for a satellite of this observation");
+ const double& elevation(azels.at(sat_number)(1));
+ auto&& obs_sat = getObservationByIdx(sat_idx_pair.second);
+
+ // check elevation
+ if (elevation < elmin)
+ {
+ //std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << elmin << std::endl;
+ remove_sats.insert(sat_number);
+ continue;
+ }
+
+ // snr L1
+ if (testsnr(0, 0, elevation, obs_sat.SNR[0] * 0.25, &snrmask) == 1)
+ {
+ //std::cout << "Discarding sat " << sat_number << ": snr test L1" << std::endl;
+ remove_sats.insert(sat_number);
+ }
+ if (multi_freq)
+ {
+ int L25 = (satsys(sat_number,NULL)&(SYS_GAL|SYS_SBS) ? 2 : 1);
+ //srn L2/L
+ if (testsnr(0, L25, elevation, obs_sat.SNR[L25] * 0.25, &snrmask) == 1)
+ {
+ //std::cout << "Discarding sat " << sat_number << ": snr test L2/L5" << std::endl;
+ remove_sats.insert(sat_number);
+ }
+ }
+ }
+
+ // remove sats
+ // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+ for (auto sat : remove_sats)
+ {
+ assert(hasSatellite(sat));
+ removeObservationBySat(sat);
+ }
+
+ return remove_sats;
+}
+
+std::set<int> Observations::filter(const Satellites& sats,
+ const std::set<int>& discarded_sats,
+ const Eigen::Vector3d& x_r,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const Options& opt)
+{
+ return filter(sats,
+ discarded_sats,
+ x_r,
+ check_code,
+ check_carrier_phase,
+ opt.getNavSys(),
+ opt.getSnrMask(),
+ opt.elmin,
+ opt.ionoopt == IONOOPT_IFLC);
+}
+
+std::set<int> Observations::filter(const Satellites& sats,
+ const std::set<int>& discarded_sats,
+ const Eigen::Vector3d& x_r,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const int& navsys,
+ const snrmask_t& snrmask,
+ const double& elmin,
+ const bool& multi_freq)
+{
+ assert(!x_r.isApprox(Eigen::Vector3d::Zero(), 1e-3));
+
+ Azels azels = computeAzels(sats, x_r);
+
+ return filter(sats,
+ discarded_sats,
+ azels,
+ check_code,
+ check_carrier_phase,
+ navsys,
+ snrmask,
+ elmin,
+ multi_freq);
+}
+
+std::set<int> Observations::filter(const Satellites& sats,
+ const std::set<int>& discarded_sats,
+ const Azels& azels,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const Options& opt)
+{
+ return filter(sats,
+ discarded_sats,
+ azels,
+ check_code,
+ check_carrier_phase,
+ opt.getNavSys(),
+ opt.getSnrMask(),
+ opt.elmin,
+ opt.ionoopt == IONOOPT_IFLC);
+}
+
+std::set<int> Observations::filter(const Satellites& sats,
+ const std::set<int>& discarded_sats,
+ const Azels& azels,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const int& navsys,
+ const snrmask_t& snrmask,
+ const double& elmin,
+ const bool& multi_freq)
+{
+ //std::cout << "filter: initial size: " << obs_.size() << std::endl;
+ // Ephemeris
+ std::set<int> remove_sats = filterByEphemeris(sats);
+
+ // Discarded sats
+ std::set<int> remove_sats_discarded = filterBySatellites(discarded_sats);
+ remove_sats.insert(remove_sats_discarded.begin(), remove_sats_discarded.end());
+
+ // Code
+ if (check_code)
+ {
+ std::set<int> remove_sats_code = filterByCode();
+ remove_sats.insert(remove_sats_code.begin(), remove_sats_code.end());
+ }
+
+ // Carrier phase
+ if (check_carrier_phase)
+ {
+ std::set<int> remove_sats_carrier = filterByCarrierPhase();
+ remove_sats.insert(remove_sats_carrier.begin(), remove_sats_carrier.end());
+ }
+
+ // satellite flag and ephemeris variance
+ for (auto sat_pair : sats)
+ if (remove_sats.count(sat_pair.first) == 0)
+ if (satexclude(sat_pair.first,sat_pair.second.var, sat_pair.second.svh, NULL))
+ {
+ //std::cout << "Discarding sat " << sat_pair.first << ": unhealthy or huge variance svh = " << sat_pair.second.svh << std::endl;
+ removeObservationBySat(sat_pair.first);
+ remove_sats.insert(sat_pair.first);
+ }
+
+ // Constellations
+ std::set<int> remove_sats_constellations = filterByConstellations(navsys);
+ remove_sats.insert(remove_sats_constellations.begin(), remove_sats_constellations.end());
+
+ // Elevation and SNR
+ std::set<int> remove_sats_elevation = filterByElevationSnr(azels, snrmask, elmin, multi_freq);
+ remove_sats.insert(remove_sats_elevation.begin(), remove_sats_elevation.end());
+
+ return remove_sats;
+ // std::cout << "final size: " << obs_.size() << std::endl;
+}
+
+std::set<int> Observations::findCommonObservations(const Observations& obs_1,
+ const Observations& obs_2)
+{
+ // std::cout << "obs 1 sats: ";
+ // for (auto&& obs_1_ref : obs_1.getObservations())
+ // std::cout << (int)obs_1_ref.sat << " ";
+ // std::cout << std::endl;
+ // std::cout << "obs 2 sats: ";
+ // for (auto&& obs_2_ref : obs_2.getObservations())
+ // std::cout << (int)obs_2_ref.sat << " ";
+ // std::cout << std::endl;
+
+ std::set<int> common_sats;
+
+ // iterate 1
+ for (auto&& obs_1_ref : obs_1.getObservations())
+ if (obs_2.hasSatellite(obs_1_ref.sat))
+ common_sats.insert(obs_1_ref.sat);
+
+ // std::cout << "common sats: ";
+ // for (auto sat : common_sats)
+ // std::cout << sat << " ";
+ // std::cout << std::endl;
+
+ return common_sats;
+}
+
+
+bool Observations::operator ==(const Observations &other_obs) const
+{
+
+ if (sat_2_idx_ != other_obs.sat_2_idx_)
+ return false;
+
+ if (idx_2_sat_ != other_obs.idx_2_sat_)
+ return false;
+
+ for (auto i = 0; i < obs_.size(); i++)
+ if (obs_.at(i) != other_obs.obs_.at(i))
+ return false;
+
+ return true;
+}
diff --git a/src/range.cpp b/src/range.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e52ee8043c0c40408365ca019af2aa296b671d85
--- /dev/null
+++ b/src/range.cpp
@@ -0,0 +1,219 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * pseudo_range.cpp
+ *
+ * Created on: May 28, 2020
+ * Author: joanvallve
+ */
+
+#include "gnss_utils/range.h"
+
+namespace GnssUtils
+{
+
+Range::Range()
+{
+ // TODO Auto-generated constructor stub
+
+}
+
+Range::~Range()
+{
+ // TODO Auto-generated destructor stub
+}
+
+Ranges Range::computeRanges(ObservationsPtr obs,
+ NavigationPtr nav,
+ const Satellites& sats,
+ const Azels& azel,
+ const Eigen::Vector3d& latlonalt,
+ const Options& opt)
+{
+ Ranges ranges;
+
+ double dion,dtrp,vmeas,vion,vtrp,P,lam_L1;
+ prcopt_t prcopt = opt.getPrcopt();
+
+ //std::cout << "compute pseudo ranges: \n";
+
+ for (auto i = 0; i<obs->size(); i++)
+ {
+ const obsd_t& obs_i(obs->getObservationByIdx(i));
+ int sat = obs_i.sat;
+ const Satellite& sat_i(sats.at(sat));
+ assert(azel.count(sat) != 0 && "azimuth and elevation not provided for this satellite");
+ double azel_i[2] = {azel.at(sat)(0),azel.at(sat)(1)};
+
+ //std::cout << "\tsat " << sat << "...";
+
+ /* ------------------- Pseudo range ------------------- */
+ /* psudorange with code bias correction */
+ //std::cout << "prange...\n";
+ if ((P=prange(&obs_i,&nav->getNavigation(),azel_i,1,&prcopt,&vmeas))==0.0)
+ {
+ //std::cout << " error in prange\n";
+ continue;
+ }
+
+ /* ionospheric corrections */
+ //std::cout << "iono...\n";
+ //std::cout << "\ttime: " << obs_i.time.time << " + " << obs_i.time.sec << "\n";
+ //std::cout << "\tnav: \n";
+ //nav->print();
+ //std::cout << "\tsat: " << sat << "\n";
+ //std::cout << "\tlatlonalt: " << latlonalt.transpose() << "\n";
+ //std::cout << "\tazel_i: " << azel_i[0] << " " << azel_i[1] << "\n";
+ //std::cout << "\topt.ionoopt: " << opt.ionoopt << "\n";
+ if (!ionocorr(obs_i.time,&nav->getNavigation(),sat,latlonalt.data(),azel_i,opt.ionoopt,&dion,&vion))
+ {
+ //std::cout << " error in iono\n";
+ continue;
+ }
+
+ /* GPS-L1 -> L1/B1 */
+ //std::cout << "iono2...\n";
+ if ((lam_L1=nav->getNavigation().lam[sat-1][0])>0.0)
+ dion*=std::pow(lam_L1/lam_carr[0],2);
+
+ /* tropospheric corrections */
+ //std::cout << "tropo...\n";
+ if (!tropcorr(obs_i.time,&nav->getNavigation(),latlonalt.data(),azel_i,opt.tropopt,&dtrp,&vtrp))
+ {
+ //std::cout << " error in tropcorr\n";
+ continue;
+ }
+
+ /* Store in PseudoRange struct */
+ // initialize with error values
+ ranges.emplace(sat,Range());
+
+ //std::cout << "storing\n";
+ ranges[sat].sat = sat;
+ ranges[sat].P = P;
+ ranges[sat].iono_corr = -dion;
+ ranges[sat].tropo_corr = -dtrp;
+ ranges[sat].sat_clock_corr = CLIGHT*sat_i.clock_bias;
+
+ /* pseudorange corrected */
+ ranges[sat].P_corrected = ranges[sat].P +
+ ranges[sat].iono_corr +
+ ranges[sat].tropo_corr +
+ ranges[sat].sat_clock_corr;
+
+ /* error variance */
+ ranges[sat].P_var = varerr(&prcopt,azel_i[1],prcopt.err[5],sat_i.sys)+sat_i.var+vmeas+vion+vtrp;
+
+ /* ------------------- Carrier phase ------------------- */
+ // L1
+ ranges[sat].L = obs_i.L[0]*nav->getNavigation().lam[sat-1][0];
+ ranges[sat].L_corrected = ranges[sat].L;
+
+ int sys = satsys(sat, NULL);
+ // L2 (GPS/GLO/QZS)
+ if (sys & (SYS_GPS | SYS_GLO | SYS_QZS))
+ ranges[sat].L2 = obs_i.L[1]*nav->getNavigation().lam[sat-1][1];
+ // E5 (GAL)
+ else if (sys & SYS_GAL)
+ ranges[sat].L2 = obs_i.L[2]*nav->getNavigation().lam[sat-1][2];
+ else
+ ranges[sat].L2 = 0;
+ ranges[sat].L2_corrected = ranges[sat].L2;
+
+ /* ionospheric corrections */
+ if (opt.carrier_opt.corr_iono)
+ {
+ if (opt.ionoopt==IONOOPT_IFLC)
+ {
+ // TODO formulation
+ //ranges[sat].L_corrected = obs_i.L[0]*nav->getNavigation().lam[sat-1][0];
+ }
+ else
+ {
+ ranges[sat].L_corrected -= ranges[sat].iono_corr;
+ ranges[sat].L2_corrected -= ranges[sat].iono_corr; //FIXME: different?
+ }
+ }
+
+ /* tropospheric corrections */
+ if (opt.carrier_opt.corr_tropo)
+ {
+ ranges[sat].L_corrected += ranges[sat].tropo_corr;
+ ranges[sat].L2_corrected += ranges[sat].tropo_corr; //FIXME: different?
+ }
+
+ /* sat clock corrections */
+ if (opt.carrier_opt.corr_clock)
+ {
+ ranges[sat].L_corrected += ranges[sat].sat_clock_corr;
+ ranges[sat].L2_corrected += ranges[sat].sat_clock_corr; //FIXME: different?
+ }
+
+ /* carrier phase variance */
+ ranges[sat].L_var = 1.0; // FIXME
+ ranges[sat].L2_var = 1.0; // FIXME
+
+ //std::cout << std::endl
+ // << "\t\tprange: " << pranges[sat].prange << std::endl
+ // << "\t\tvar: " << pranges[sat].var << std::endl
+ // << "\t\tP: " << P << std::endl
+ // << "\t\tsat_i.clock_bias: " << sat_i.clock_bias << std::endl
+ // << "\t\tdion: " << dion << std::endl
+ // << "\t\tdtrp: " << dtrp << std::endl;
+ }
+
+ return ranges;
+}
+
+
+
+std::set<int> Range::findCommonSatellites(const Ranges& ranges_1, const Ranges& ranges_2)
+{
+ std::set<int> common_sats;
+
+ // std::cout << "ranges_1: ";
+ // for (auto&& range_pair : ranges_1)
+ // std::cout << range_pair.first << " ";
+ // std::cout << std::endl;
+ // std::cout << "ranges_2: ";
+ // for (auto&& range_pair : ranges_2)
+ // std::cout << range_pair.first << " ";
+ // std::cout << std::endl;
+
+ for (auto&& range_pair : ranges_1)
+ if (ranges_2.count(range_pair.first))
+ common_sats.insert(range_pair.first);
+
+ // std::cout << "common sats: ";
+ // for (auto sat : common_sats)
+ // std::cout << sat << " ";
+ // std::cout << std::endl;
+
+ return common_sats;
+}
+
+double computeSagnacCorrection(const Eigen::Vector3d& rcv_ECEF, const Eigen::Vector3d& sat_ECEF)
+{
+ return OMGE*(sat_ECEF(0)*rcv_ECEF(1)-sat_ECEF(1)*rcv_ECEF(0))/CLIGHT;
+}
+
+} /* namespace GnssUtils */
diff --git a/src/receiver_raw_base.cpp b/src/receiver_raw_base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f21b8e6d05b1fd0a68a333aad2a0abe2132d858a
--- /dev/null
+++ b/src/receiver_raw_base.cpp
@@ -0,0 +1,50 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gnss_utils/receiver_raw_base.h"
+
+namespace GnssUtils
+{
+ReceiverRawAbstract::ReceiverRawAbstract() : raw_data_type_(NO)
+{
+ if (init_raw(&raw_data_, STRFMT_UBX) == 0)
+ {
+ assert("Failed when allocating memory for raw_t");
+ return;
+ }
+}
+
+ReceiverRawAbstract::~ReceiverRawAbstract()
+{
+ free_raw(&raw_data_);
+}
+
+void ReceiverRawAbstract::updateObservations()
+{
+ // sort and remove duplicated observations
+ sortobs(&raw_data_.obs);
+
+ obs_.clearObservations();
+ for (int ii = 0; ii < raw_data_.obs.n; ++ii)
+ obs_.addObservation(raw_data_.obs.data[ii]);
+}
+
+} // namespace GnssUtils
diff --git a/src/receivers/novatel_raw.cpp b/src/receivers/novatel_raw.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aa4de46b21b6ed078c9f12a30eb622aced2201ca
--- /dev/null
+++ b/src/receivers/novatel_raw.cpp
@@ -0,0 +1,90 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gnss_utils/receivers/novatel_raw.h"
+
+using namespace GnssUtils;
+
+NovatelRaw::NovatelRaw()
+{
+}
+
+NovatelRaw::~NovatelRaw()
+{
+}
+
+RawDataType NovatelRaw::addDataStream(const std::vector<uint8_t>& data_stream)
+{
+
+ for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte)
+ {
+ raw_data_type_ = static_cast<RawDataType> ( input_oem4(&raw_data_, (unsigned char)*data_byte) );
+
+ switch (raw_data_type_)
+ {
+ case NO: //
+ // std::cout << "0 received!\n";
+ break;
+
+ case OBS: // Observations
+ // std::cout << "Observations Novatel received!\n";
+ updateObservations();
+ break;
+
+ case NAV_EPH: // Ephemeris
+ //std::cout << "Ephemeris received!\n";
+ nav_.copyEphemeris(raw_data_.nav);
+ nav_.uniqueNavigation();
+ break;
+
+ case NAV_SBAS: // SBAS
+ //std::cout << "SBAS received!\n";
+ nav_.copySbasCorrections(raw_data_.nav);
+ break;
+
+ case NAV_ALM_IONUTC: // Almanac and ion/utc parameters
+ //std::cout << "Almanac and ion/utc parameters received!\n";
+ nav_.copyIonUtc(raw_data_.nav);
+ break;
+
+ // Not handled messages
+ case NAV_ANT:
+ //std::cout << "NovatelRaw: Received antenna postion parameters. Not handled.\n";
+ break;
+ case NAV_DGPS:
+ //std::cout << "NovatelRaw: Received dgps correction. Not handled.\n";
+ break;
+ case NAV_SSR:
+ //std::cout << "NovatelRaw: Received ssr message. Not handled.\n";
+ break;
+ case NAV_LEX:
+ //std::cout << "NovatelRaw: Received lex message. Not handled.\n";
+ break;
+ case ERROR:
+ //std::cout << "NovatelRaw: Received error message. Not handled.\n";
+ break;
+ default:
+ //std::cout << "NovatelRaw: Received unknown message. Not handled.\n";
+ break;
+ }
+ }
+ return raw_data_type_;
+}
diff --git a/src/receivers/ublox_raw.cpp b/src/receivers/ublox_raw.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b6e03fee2b5d76a9e85172e8dbf403588da79356
--- /dev/null
+++ b/src/receivers/ublox_raw.cpp
@@ -0,0 +1,94 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gnss_utils/receivers/ublox_raw.h"
+
+using namespace GnssUtils;
+
+UBloxRaw::UBloxRaw()
+{
+}
+
+UBloxRaw::~UBloxRaw()
+{
+}
+
+RawDataType UBloxRaw::addDataStream(const std::vector<uint8_t>& data_stream)
+{
+ // Update type based on RTKLIB
+ for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte)
+ {
+ raw_data_type_ = static_cast<RawDataType>(input_ubx(&raw_data_, (unsigned char)*data_byte));
+
+ switch (raw_data_type_)
+ {
+ case NO: //
+ // std::cout << "0 received!\n";
+ break;
+
+ case OBS: // Observations
+ // std::cout << "Observations received!\n";
+ updateObservations();
+ break;
+
+ case NAV_EPH: // Ephemeris
+ //std::cout << "Ephemeris received!\n";
+ nav_.copyEphemeris(raw_data_.nav);
+ break;
+
+ case NAV_SBAS: // SBAS
+ //std::cout << "SBAS received!\n";
+ if (raw_data_.sbsmsg.prn == 123 or raw_data_.sbsmsg.prn == 136) // only EGNOS corrections
+ nav_.addSbasMessage(raw_data_.sbsmsg);
+ // std::cout << "SBAS added!\n";
+ break;
+
+ case NAV_ALM_IONUTC: // Almanac and ion/utc parameters
+ //std::cout << "Almanac and ion/utc parameters received!\n";
+ nav_.freeAlmanac();
+ nav_.copyAlmanac(raw_data_.nav);
+ nav_.copyIonUtc(raw_data_.nav);
+ break;
+
+ // Not handled messages
+ case NAV_ANT:
+ //std::cout << "UBloxRaw: Received antenna postion parameters. Not handled.\n";
+ break;
+ case NAV_DGPS:
+ //std::cout << "UBloxRaw: Received dgps correction. Not handled.\n";
+ break;
+ case NAV_SSR:
+ //std::cout << "UBloxRaw: Received ssr message. Not handled.\n";
+ break;
+ case NAV_LEX:
+ //std::cout << "UBloxRaw: Received lex message. Not handled.\n";
+ break;
+ case ERROR:
+ //std::cout << "UBloxRaw: Received error message. Not handled.\n";
+ break;
+ default:
+ //std::cout << "UBloxRaw: Received unknown message. Not handled.\n";
+ break;
+ }
+ }
+
+ return raw_data_type_;
+}
diff --git a/src/snapshot.cpp b/src/snapshot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9de0e176dbf69b719617016e4b4c44cb8315eb4f
--- /dev/null
+++ b/src/snapshot.cpp
@@ -0,0 +1,74 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gnss_utils/snapshot.h"
+#include "gnss_utils/utils/satellite.h"
+#include "gnss_utils/utils/transformations.h"
+
+using namespace GnssUtils;
+
+Snapshot::Snapshot()
+ : obs_(nullptr)
+ , nav_(nullptr)
+{
+ //
+}
+
+Snapshot::Snapshot(ObservationsPtr obs, NavigationPtr nav)
+ : obs_(obs)
+ , nav_(nav)
+{
+ assert(obs_ != nullptr);
+ assert(nav_ != nullptr);
+ //
+}
+
+void Snapshot::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt)
+{
+ throw std::runtime_error("not implemented!");
+}
+
+void Snapshot::computeSatellites(const int& eph_opt)
+{
+ assert(obs_!=nullptr && "null obs");
+ assert(nav_!=nullptr && "null nav");
+ //assert(!satellitesComputed() && "satellites positions already computed");
+
+ sats_ = GnssUtils::computeSatellites(*obs_, *nav_, eph_opt);
+}
+
+void Snapshot::computeRanges(const Azels& azel,
+ const Eigen::Vector3d& latlonalt,
+ const Options& opt)
+{
+ //assert(!rangesComputed() && "pseudo ranges already computed!");
+
+ ranges_ = Range::computeRanges(obs_, nav_, sats_, azel, latlonalt, opt);
+}
+
+void Snapshot::computeRanges(const Eigen::Vector3d& x_ecef,
+ const Options& opt)
+{
+ ranges_ = Range::computeRanges(obs_, nav_, sats_,
+ computeAzels(sats_, x_ecef),
+ ecefToLatLonAlt(x_ecef),
+ opt);
+}
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cb4fc40a7908bca237da9a3d5567e5b06a206af3
--- /dev/null
+++ b/src/tdcp.cpp
@@ -0,0 +1,659 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include <iomanip>
+#include "gnss_utils/tdcp.h"
+#include "gnss_utils/utils/rcv_position.h"
+#include "gnss_utils/utils/satellite.h"
+#include "gnss_utils/snapshot.h"
+#include "gnss_utils/utils/chisquare_ci.h"
+
+namespace GnssUtils
+{
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector4d& d_0,
+ const std::set<int>& discarded_sats,
+ const TdcpOptions& tdcp_params,
+ const Options& opt)
+{
+ auto pos_output = computePos(*snapshot_r->getObservations(), *snapshot_r->getNavigation(), opt);
+
+ if (not pos_output.success)
+ {
+ TdcpOutput output;
+ output.success = false;
+ output.msg = "Couldn't compute fix for snapshot_r: " + pos_output.msg;
+ return output;
+ }
+
+ return Tdcp(snapshot_r,
+ snapshot_k,
+ pos_output.pos,
+ d_0,
+ discarded_sats,
+ tdcp_params,
+ opt);
+}
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector3d& x_r,
+ const Eigen::Vector4d& d_0,
+ const std::set<int>& discarded_sats,
+ const TdcpOptions& tdcp_params,
+ const Options& opt)
+{
+ return Tdcp(snapshot_r,
+ snapshot_k,
+ x_r,
+ d_0,
+ discarded_sats,
+ std::set<int>({}),
+ tdcp_params,
+ opt);
+}
+
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector3d& x_r,
+ const Eigen::Vector4d& d_0,
+ const std::set<int>& discarded_sats,
+ const std::set<int>& tracked_sats,
+ const TdcpOptions& tdcp_params,
+ const Options& opt)
+{
+ // If use old nav temporary change navigation to (re)compute satellites positions
+ auto nav_k = snapshot_k->getNavigation();
+ if (tdcp_params.use_old_nav)
+ {
+ auto new_snapshot_k = std::make_shared<Snapshot>(std::make_shared<Observations>(*snapshot_k->getObservations()),
+ std::make_shared<Navigation>(*snapshot_r->getNavigation()));
+ snapshot_k = new_snapshot_k;
+ }
+
+ // COMPUTE SATELLITES POSITION
+ if (not snapshot_r->satellitesComputed())
+ snapshot_r->computeSatellites(opt.sateph);
+ if (not snapshot_k->satellitesComputed())
+ snapshot_k->computeSatellites(opt.sateph);
+
+ // FILTER SATELLITES (ALREADY DISCARDED, CORRUPTED DATA, WRONG POSITION, CONSTELLATION, ELEVATION and SNR)
+ snapshot_r->filterObservations(discarded_sats, x_r, false, true, opt);
+ snapshot_k->filterObservations(discarded_sats, x_r, false, true, opt);
+
+ // COMPUTE RANGES
+ if (not snapshot_r->rangesComputed())
+ snapshot_r->computeRanges(x_r, opt);
+ if (not snapshot_k->rangesComputed())
+ snapshot_k->computeRanges(x_r, opt); // the x_r is only used to compute Azels -> iono&tropo corrections
+
+ // FIND COMMON SATELLITES
+ std::set<int> common_sats = Range::findCommonSatellites(snapshot_r->getRanges(),
+ snapshot_k->getRanges());
+
+ // FILTER OUT SATELLITES NOT IN tracked_sats (if not empty)
+ if (not tracked_sats.empty())
+ {
+ auto com_sat_it = common_sats.begin();
+ while (com_sat_it != common_sats.end())
+ if (tracked_sats.count(*com_sat_it) == 0)
+ com_sat_it = common_sats.erase(com_sat_it);
+ else
+ com_sat_it++;
+ }
+
+ if (common_sats.empty())
+ {
+ TdcpOutput output;
+ output.success = false;
+ output.msg = "No common satellites after filtering observations.";
+ return output;
+ }
+
+ // COMPUTE TDCP
+ return Tdcp(snapshot_r,
+ snapshot_k,
+ x_r,
+ common_sats,
+ d_0,
+ tdcp_params);
+}
+
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector3d& x_r,
+ const std::set<int>& common_sats,
+ const Eigen::Vector4d& d_0,
+ const TdcpOptions& tdcp_params)
+{
+
+ TdcpOutput output;
+
+ assert(snapshot_r->satellitesComputed() && "this TDCP assumes satellites already computed");
+ assert(snapshot_k->satellitesComputed() && "this TDCP assumes satellites already computed");
+ assert(snapshot_r->rangesComputed() && "this TDCP assumes ranges already computed");
+ assert(snapshot_k->rangesComputed() && "this TDCP assumes ranges already computed");
+ assert(snapshot_r->getSatellites().size() >= common_sats.size() && "Too many common sats");
+ assert(snapshot_k->getSatellites().size() >= common_sats.size() && "Too many common sats");
+ assert(snapshot_r->getRanges().size() >= common_sats.size() && "Too many common sats");
+ assert(snapshot_k->getRanges().size() >= common_sats.size() && "Too many common sats");
+ for (auto sat : common_sats)
+ {
+ assert(snapshot_r->getSatellites().count(sat) && "common sat not stored in satellites");
+ assert(snapshot_k->getSatellites().count(sat) && "common sat not stored in satellites");
+ assert(snapshot_r->getRanges().count(sat) && "common sat not stored in ranges");
+ assert(snapshot_k->getRanges().count(sat) && "common sat not stored in ranges");
+ }
+
+ int n_common_sats = common_sats.size();
+ if (n_common_sats < std::max(tdcp_params.min_common_sats, 4))
+ {
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ printf("Tdcp: NOT ENOUGH COMMON SATS\n");
+ printf("Tdcp: %i common sats available, %i sats are REQUIRED. [ SKIPPING Tdcp ]\n", n_common_sats, std::max(tdcp_params.min_common_sats, 4));
+ #endif
+ output.msg = "Not enough common sats";
+ output.success = false;
+ return output;
+ }
+
+ // Times
+ double tr = snapshot_r->getGPST();
+ double tk = snapshot_k->getGPST();
+
+ // MULTI-FREQUENCY
+ std::map<int, std::pair<int, int>> row_2_sat_freq;
+ int row = 0;
+ for (auto sat : common_sats)
+ {
+ assert(snapshot_r->getRanges().count(sat));
+ assert(snapshot_k->getRanges().count(sat));
+
+ auto&& range_r = snapshot_r->getRanges().at(sat);
+ auto&& range_k = snapshot_k->getRanges().at(sat);
+
+ // Carrier phase
+ // L1/E1
+ if (std::abs(range_r.L_corrected) > 1e-12 and std::abs(range_k.L_corrected) > 1e-12)
+ row_2_sat_freq[row++] = std::make_pair(sat, 0);
+
+ if (!tdcp_params.multi_freq)
+ continue;
+
+ // L2 (GPS/GLO/QZS) / E5 (GAL)
+ if (std::abs(range_r.L2_corrected) > 1e-12 and std::abs(range_k.L2_corrected) > 1e-12)
+ row_2_sat_freq[row++] = std::make_pair(sat, 1);
+ }
+ int n_differences = row_2_sat_freq.size();
+
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
+ std::cout << "d initial guess: " << d_0.transpose() << std::endl;
+ std::cout << "common sats: ";
+ for (auto row_sat_freq_it : row_2_sat_freq)
+ std::cout << row_sat_freq_it.second.first << " ";
+ std::cout << std::endl;
+ #endif
+
+ // FILL SATELLITES POSITIONS AND MEASUREMENTS =======================================================================
+ Eigen::MatrixXd A(Eigen::MatrixXd::Zero(n_differences, 4));
+ Eigen::VectorXd r(Eigen::VectorXd::Zero(n_differences));
+ Eigen::VectorXd drho_m(Eigen::VectorXd::Zero(n_differences));
+ Eigen::MatrixXd s_k(Eigen::MatrixXd::Zero(3, n_differences));
+ Eigen::MatrixXd s_r(Eigen::MatrixXd::Zero(3, n_differences));
+
+ for (auto row_sat_freq_it : row_2_sat_freq)
+ {
+ const int& row = row_sat_freq_it.first;
+ const int& sat_number = row_sat_freq_it.second.first;
+ const int& freq = row_sat_freq_it.second.second;
+
+ auto&& range_r = snapshot_r->getRanges().at(sat_number);
+ auto&& range_k = snapshot_k->getRanges().at(sat_number);
+
+ // Satellite's positions
+ s_r.col(row) = snapshot_r->getSatellites().at(sat_number).pos;
+ s_k.col(row) = snapshot_k->getSatellites().at(sat_number).pos;
+
+ // time differenced carrier phase measurements
+ if (freq == 0)
+ drho_m(row) = range_k.L_corrected - range_r.L_corrected;
+ else
+ drho_m(row) = range_k.L2_corrected - range_r.L2_corrected;
+
+ // sagnac corrections
+ if (tdcp_params.sagnac_correction != 0)
+ {
+ double sagnac_corr_r = computeSagnacCorrection(x_r,s_r.col(row)); //OMGE*(s_r.col(row)(0)*x_r(1)-s_r.col(row)(1)*x_r(0))/CLIGHT;
+ double sagnac_corr_k = computeSagnacCorrection(x_r+d_0.head(3),s_k.col(row)); //OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT;
+
+ drho_m(row) += -sagnac_corr_k + sagnac_corr_r;
+ }
+
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "\tsat " << sat_number;
+ std::cout << std::setprecision(10)
+ << "\tpositions:" << std::endl
+ << "\t\ts_r: " << s_r.col(row).transpose() << std::endl
+ << "\t\ts_k: " << s_k.col(row).transpose() << std::endl;
+ std::cout << "\trange_r.L: " << (freq == 0 ? range_r.L_corrected : range_r.L2_corrected) << std::endl;
+ std::cout << "\trange_k.L: " << (freq == 0 ? range_k.L_corrected : range_k.L2_corrected) << std::endl;
+ std::cout << "\tdrho_m: " << drho_m(row) << std::endl;
+ #endif
+
+ if (!tdcp_params.relinearize_jacobian)
+ {
+ // Unit vectors from receivers to satellites
+ Eigen::Vector3d u_k = (s_k.col(row) - x_r - d_0.head(3)).normalized();
+
+ // Fill Jacobian matrix
+ A(row, 0) = u_k(0);
+ A(row, 1) = u_k(1);
+ A(row, 2) = u_k(2);
+ A(row, 3) = -1.0;
+ }
+ }
+ // Compute TDCP measurement noise variance (proportional to time)
+ double var_tdcp = (tk - tr) * tdcp_params.sigma_atm * tdcp_params.sigma_atm +
+ 2 * tdcp_params.sigma_carrier * tdcp_params.sigma_carrier;
+
+ // LEAST SQUARES SOLUTION =======================================================================
+ std::set<int> raim_discarded_rows;
+ output = Tdcp(x_r, A, r, drho_m, s_k, s_r, d_0, var_tdcp, raim_discarded_rows, tdcp_params);
+
+ // FILL OUTPUT
+ output.used_sats = common_sats;
+ for (auto disc_row : raim_discarded_rows)
+ {
+ output.raim_discarded_sats.insert(row_2_sat_freq[disc_row].first);
+ output.used_sats.erase(row_2_sat_freq[disc_row].first);
+ }
+ output.dt = tk - tr;
+
+ return output;
+}
+
+TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
+ Eigen::MatrixXd& A,
+ Eigen::VectorXd& r,
+ Eigen::VectorXd& drho_m,
+ const Eigen::MatrixXd& s_k,
+ const Eigen::MatrixXd& s_r,
+ const Eigen::Vector4d& d_0,
+ const double& var_tdcp,
+ std::set<int>& raim_discarded_rows,
+ const TdcpOptions& tdcp_params)
+{
+ assert(A.cols() == 4);
+ assert(s_k.rows() == 3);
+ assert(s_r.rows() == 3);
+
+ TdcpOutput output;
+ Eigen::Vector4d& d(output.d);
+ Eigen::Matrix4d& cov_d(output.cov_d);
+ double& residual(output.residual);
+ double& residual_ci(output.residual_ci);
+
+ const int n_rows = A.rows();
+
+ // Initial guess
+ Eigen::Vector4d delta_d(Eigen::Vector4d::Zero());
+ d = d_0;
+
+ // initial sagnac correction
+ Eigen::VectorXd prev_sagnac_corr(s_k.cols());
+ if (tdcp_params.sagnac_correction == 2 )
+ for (int row = 0; row < n_rows; row++)
+ {
+ double sagnac_corr_r = computeSagnacCorrection(x_r,s_r.col(row)); //OMGE*(s_r.col(row)(0)*x_r(1)-s_r.col(row)(1)*x_r(0))/CLIGHT;
+ double sagnac_corr_k = computeSagnacCorrection(x_r+d_0.head(3),s_k.col(row)); //OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT;
+
+ prev_sagnac_corr(row) = -sagnac_corr_k + sagnac_corr_r;
+ }
+
+ for (int j = 0; j < tdcp_params.max_iterations; j++)
+ {
+ // fill A and r
+ for (int row = 0; row < n_rows; row++)
+ {
+ // skip discarded rows
+ if (raim_discarded_rows.count(row) != 0)
+ continue;
+
+ // update sagnac correction
+ if (tdcp_params.sagnac_correction == 2 and j > 0)
+ {
+ // undo prev_correction
+ drho_m(row) -= prev_sagnac_corr(row);
+
+ // apply new correction
+ double sagnac_corr_r = computeSagnacCorrection(x_r,s_r.col(row)); //OMGE*(s_r.col(row)(0)*x_r(1)-s_r.col(row)(1)*x_r(0))/CLIGHT;
+ double sagnac_corr_k = computeSagnacCorrection(x_r+d.head(3),s_k.col(row)); //OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT;
+ drho_m(row) += -sagnac_corr_k + sagnac_corr_r;
+
+ // store correction
+ prev_sagnac_corr(row) = -sagnac_corr_k + sagnac_corr_r;
+ }
+
+ // Evaluate r
+ r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
+
+ // Evaluate A
+ if (tdcp_params.relinearize_jacobian)
+ {
+ // Unit vectors from rcvrs to sats
+ Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
+
+ // Fill Jacobian matrix
+ A(row, 0) = u_k(0);
+ A(row, 1) = u_k(1);
+ A(row, 2) = u_k(2);
+ A(row, 3) = -1.0;
+ }
+ }
+
+ // Solve
+ cov_d = (A.transpose() * A).inverse();
+ delta_d = -cov_d * A.transpose() * r;
+ d += delta_d;
+
+ // wrong solution
+ if (d.array().isNaN().any())
+ {
+ std::cout << "Tdcp: NLS DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "Displacement vector =" << d.head<3>().transpose() << std::endl;
+ std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl;
+ printf("Ref distance = %7.3f m\n", d_0.norm());
+ printf("Computed distance = %7.3f m\n", d.head<3>().norm());
+ printf("Tdcp: rows = %lu\n", n_rows);
+ std::cout << "Tdcp: r = " << r.transpose() << std::endl;
+ std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl;
+ std::cout << "Tdcp: A = \n" << A << std::endl;
+ std::cout << "Tdcp: H = \n" << A.transpose() * A << std::endl;
+ std::cout << "Tdcp: cov_d = \n" << cov_d << std::endl;
+ printf("Tdcp: dT = %8.3f secs\n", d(3));
+ #endif
+ output.msg = "Na"
+ "N values in NLS";
+ output.success = false;
+ return output;
+ }
+
+ // residual
+ if (tdcp_params.residual_opt == 0) // all problem
+ residual = (r + A * delta_d).squaredNorm() / var_tdcp;
+ else if (tdcp_params.residual_opt == 1) // worst sat
+ residual = (r + A * delta_d).cwiseAbs2().maxCoeff() / var_tdcp;
+ else
+ throw std::runtime_error("unknown value for residual_opt");
+ // residual = (r + A * delta_d).norm();
+
+ // residual_ci
+ residual_ci = chisq2ci(residual, tdcp_params.residual_opt == 0 ? n_rows : 1);
+
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ if (j == 0)
+ {
+ std::cout << "\n//////////////// Initial solution at first iteration ////////////////\n";
+ std::cout << "Displacement vector =" << d.head<3>().transpose() << std::endl;
+ std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl;
+ printf("Ref distance = %7.3f m\n", d_0.norm());
+ printf("Computed distance = %7.3f m\n", d.head<3>().norm());
+ printf("Tdcp: residual = %13.10f\n", residual);
+ printf("Tdcp: residual_ci = %13.10f\n", residual_ci);
+ printf("Tdcp: rows = %lu\n", n_rows);
+ std::cout << "Tdcp: r = " << r.transpose() << std::endl;
+ std::cout << "Tdcp: r+A*delta_d = " << (r + A * delta_d).transpose() << std::endl;
+ std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl;
+ std::cout << "Tdcp: A = \n" << A << std::endl;
+ std::cout << "Tdcp: H = \n" << A.transpose() * A << std::endl;
+ std::cout << "Tdcp: cov_d = \n" << cov_d << std::endl;
+ printf("Tdcp: dT = %8.3f secs\n", d(3));
+ std::cout << "Check RAIM conditions...\n";
+ std::cout << "\titeration == 0: " << (j==0 ? "OK\n":"FAILED\n");
+ std::cout << "\tn_rows > tdcp_params.min_common_sats: " << (n_rows > tdcp_params.min_common_sats ? "OK\n":"FAILED\n");
+ std::cout << "\ttdcp_params.raim_n > 0: " << (tdcp_params.raim_n > 0 ? "OK\n":"FAILED\n");
+ std::cout << "\ttdcp_params.max_residual_ci: " << tdcp_params.max_residual_ci;
+ }
+ #endif
+
+ // RAIM ====================================== (after first iteration)
+ if (j == 0 and
+ n_rows > tdcp_params.min_common_sats and
+ tdcp_params.raim_n > 0 and
+ residual_ci > tdcp_params.max_residual_ci)
+ {
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "//////////////// Performing RAIM ////////////////\n";
+ #endif
+
+ int worst_sat_row = -1;
+ double best_residual = 1e12;
+ Eigen::Vector4d best_d;
+
+ // remove up to 'raim_n' observations (if enough satellites and residual condition holds)
+ while (raim_discarded_rows.size() < tdcp_params.raim_n and
+ n_rows - raim_discarded_rows.size() > tdcp_params.min_common_sats and
+ residual_ci > tdcp_params.max_residual_ci)
+ {
+ auto A_raim = A;
+ auto r_raim = r;
+
+ // METHOD A: using normalized RMSE residual
+ if (tdcp_params.residual_opt == 0)
+ {
+ // solve removing each row
+ for (int row_removed = 0; row_removed < n_rows; row_removed++)
+ {
+ // skip already discarded rows
+ if (raim_discarded_rows.count(row_removed) != 0)
+ continue;
+
+ // remove row
+ A_raim.row(row_removed) = Eigen::Vector4d::Zero().transpose();
+ r_raim(row_removed) = 0;
+
+ // solve
+ Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
+ Eigen::Vector4d d_raim = d_0 + delta_d_raim;
+
+ // no NaN
+ if (!d_raim.array().isNaN().any())
+ {
+ // residual
+ if (tdcp_params.residual_opt == 0) // all problem
+ residual = (r_raim + A_raim * delta_d_raim).squaredNorm() / var_tdcp;
+ else if (tdcp_params.residual_opt == 1) // worst sat
+ residual = (r_raim + A_raim * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp;
+ else
+ throw std::runtime_error("unknown value for residual_opt");
+ // residual = (r_raim + A_raim * delta_d_raim).norm() / (n_rows - 1);
+
+ // residual_ci
+ residual_ci = chisq2ci(residual, tdcp_params.residual_opt == 0 ? n_rows - raim_discarded_rows.size() : 1);
+
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "RAIM excluding row " << row_removed;// << std::endl;
+ //std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << std::endl;
+ printf("\tresidual = %13.10f\n", residual);
+ printf("\tresidual_ci = %13.10f\n", residual_ci);
+ //std::cout << "Tdcp: drho = " << r_raim.transpose() << std::endl;
+ //std::cout << "Tdcp: A = \n" << A_raim << std::endl;
+ //std::cout << "Tdcp: H = \n" << A_raim.transpose() * A_raim << std::endl;
+ //printf("Tdcp: dT = %8.3f secs\n", d_raim(3));
+ #endif
+
+ // store if best residual
+ if (residual < best_residual)
+ {
+ worst_sat_row = row_removed;
+ best_residual = residual;
+ best_d = d_raim;
+ }
+ }
+ // restore initial A and r
+ A_raim.row(row_removed) = A.row(row_removed);
+ r_raim(row_removed) = r(row_removed);
+ }
+ }
+ // METHOD B: using max residual in Mahalanobis distance
+ else if (tdcp_params.residual_opt == 1)
+ {
+ // find index of max residual
+ (r + A * delta_d).cwiseAbs2().maxCoeff(&worst_sat_row);
+
+ // remove row
+ A_raim.row(worst_sat_row) = Eigen::Vector4d::Zero().transpose();
+ r_raim(worst_sat_row) = 0;
+
+ // solve
+ Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
+ Eigen::Vector4d d_raim = d_0 + delta_d_raim;
+
+ // no NaN
+ if (!d_raim.array().isNaN().any())
+ {
+ // residual
+ if (tdcp_params.residual_opt == 0) // all problem
+ residual = (r_raim + A_raim * delta_d_raim).squaredNorm() / var_tdcp;
+ else if (tdcp_params.residual_opt == 1) // worst sat
+ residual = (r_raim + A_raim * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp;
+ else
+ throw std::runtime_error("unknown value for residual_opt");
+ // residual = (r_raim + A_raim * delta_d_raim).norm() / (n_rows - 1);
+
+ // residual_ci
+ residual_ci = chisq2ci(residual, tdcp_params.residual_opt == 0 ? n_rows - raim_discarded_rows.size() : 1);
+
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "RAIM excluding row " << worst_sat_row;// << std::endl;
+ //std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << std::endl;
+ printf("\tresidual = %13.10f\n", residual);
+ printf("\tresidual_ci = %13.10f\n", residual_ci);
+ //std::cout << "Tdcp: drho = " << r_raim.transpose() << std::endl;
+ //std::cout << "Tdcp: A = \n" << A_raim << std::endl;
+ //std::cout << "Tdcp: H = \n" << A_raim.transpose() * A_raim << std::endl;
+ //printf("Tdcp: dT = %8.3f secs\n", d_raim(3));
+ #endif
+
+ // store as best residual
+ best_residual = residual;
+ best_d = d_raim;
+ }
+ }
+ else
+ throw std::runtime_error("unknown value for 'residual_opt'");
+
+ // No successful RAIM solution
+ if (worst_sat_row == -1)
+ {
+ printf("Tdcp: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!\n");
+ output.msg = "NaN values in NLS after RAIM";
+ output.success = false;
+ return output;
+ }
+
+ // store removed sat
+ raim_discarded_rows.insert(worst_sat_row);
+
+ // remove row (set zero)
+ A.row(worst_sat_row) = Eigen::Vector4d::Zero().transpose();
+ r(worst_sat_row) = 0;
+
+ // Choose the best RAIM solution
+ d = best_d;
+ cov_d = (A.transpose() * A).inverse();
+ residual= best_residual;
+ }
+
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "//////////////// Best solution after RAIM ////////////////" << std::endl;
+ std::cout << "\tExcluded rows : ";
+ for (auto excl_row : raim_discarded_rows)
+ std::cout << excl_row << " ";
+ std::cout << std::endl;
+ std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tClock offset = " << d(3) << std::endl;
+ std::cout << "\tResidual = " << best_residual << std::endl;
+ std::cout << "\tA = \n" << A << std::endl;
+ std::cout << "\tH = \n" << A.transpose() * A << std::endl;
+ std::cout << "\tcov_d = \n" << cov_d << std::endl;
+ #endif
+ }
+
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "\n//////////////// Final solution at iteration " << j << "////////////////\n";
+ std::cout << "\tExcluded rows : ";
+ for (auto excl_row : raim_discarded_rows)
+ std::cout << excl_row << " ";
+ std::cout << std::endl;
+ std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tClock offset = " << d(3) << std::endl;
+ std::cout << "\tResidual = " << residual << std::endl;
+ std::cout << "\tResidual_ci = " << residual_ci << std::endl;
+ std::cout << "\tA = \n" << A << std::endl;
+ std::cout << "\tH = \n" << A.transpose() * A << std::endl;
+ std::cout << "\tcov_d = \n" << cov_d << std::endl;
+ #endif
+ }
+
+ // Computing error on measurement space
+ r.setZero();
+ for (int row = 0; row < r.size(); row++)
+ if (raim_discarded_rows.count(row) == 0)
+ r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
+ // residual
+ if (tdcp_params.residual_opt == 0) // all problem
+ residual = r.squaredNorm() / var_tdcp;
+ else if (tdcp_params.residual_opt == 1) // worst sat
+ residual = r.cwiseAbs2().maxCoeff() / var_tdcp;
+ else
+ throw std::runtime_error("unknown value for residual_opt");
+ //residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size()));
+
+ // residual_ci
+ residual_ci = chisq2ci(residual, tdcp_params.residual_opt == 0 ? n_rows - raim_discarded_rows.size() : 1);
+
+ // check residual condition
+ if (tdcp_params.validate_residual and
+ residual_ci > tdcp_params.max_residual_ci)
+ {
+ printf("Tdcp: Didn't success. Final residual=%f (CI %f) bigger than max_residual_ci=%f.\n",
+ residual,
+ residual_ci,
+ tdcp_params.max_residual_ci);
+ output.msg = "residual_ci bigger than max_residual_ci";
+ output.success = false;
+ }
+ else
+ output.success = true;
+
+ // covariance
+ cov_d *= var_tdcp;
+
+ return output;
+}
+
+
+} // namespace GnssUtils
diff --git a/src/utils/rcv_position.cpp b/src/utils/rcv_position.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b354333ac35c49054894c7a31382c91552d1a05c
--- /dev/null
+++ b/src/utils/rcv_position.cpp
@@ -0,0 +1,314 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * transfromation.h
+ *
+ * Created on: April 3, 2020
+ * Author: Joan Vallvé, Pep MartÃ-Saumell
+ */
+
+#include "gnss_utils/utils/rcv_position.h"
+
+using namespace GnssUtils;
+
+namespace GnssUtils
+{
+ComputePosOutput computePos(const GnssUtils::Observations& _observations,
+ const GnssUtils::Navigation& _navigation,
+ const Options& _opt)
+{
+ // Convert options to rtklib
+ prcopt_t prcopt = _opt.getPrcopt();
+
+ return computePos(_observations, _navigation, prcopt);
+}
+
+ComputePosOutput computePos(const GnssUtils::Observations& _observations,
+ const GnssUtils::Navigation& _navigation,
+ const prcopt_t& _prcopt)
+{
+ // Define error msg
+ char msg[128] = "";
+
+ // output data
+ GnssUtils::ComputePosOutput output;
+ sol_t sol;
+ sol = { { 0 } };
+ ssat_t sats_status[MAXSAT];
+ std::vector<double> sat_elevations(2 * _observations.size());
+
+ // compute pose
+ output.success = pntpos(_observations.data(),
+ _observations.size(),
+ &(_navigation.getNavigation()),
+ &_prcopt,
+ &sol,
+ sat_elevations.data(),
+ sats_status,
+ msg);
+
+ // Fill output struct
+ output.msg = std::string(msg);
+ output.time = sol.time.time;
+ output.sec = sol.time.sec;
+ output.pos = Eigen::Vector3d(sol.rr);
+ output.vel = Eigen::Vector3d(&sol.rr[3]);
+ output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5], sol.qr[3], sol.qr[1], sol.qr[4], sol.qr[5], sol.qr[4], sol.qr[2];
+ // std::cout << "Compute pos: " << output.pos.transpose() << "\n";
+ // std::cout << "Covariance:\n" << output.pos_covar << "\n";
+
+ if (sol.dtr != NULL)
+ output.rcv_bias =
+ (Eigen::Matrix<double, 6, 1>() << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5])
+ .finished();
+ output.type = sol.type;
+ output.stat = sol.stat;
+ output.ns = sol.ns;
+ output.age = sol.age;
+ output.ratio = sol.ratio;
+ output.lat_lon = ecefToLatLonAlt(output.pos);
+
+ for (auto i = 0; i < _observations.size(); i++)
+ output.sat_azel.emplace(_observations.getObservationByIdx(i).sat,
+ Eigen::Vector2d(sat_elevations.at(2 * i), sat_elevations.at(2 * i + 1)));
+
+ for (auto i = 0; i < MAXSAT; i++)
+ {
+ int sat = i+1;
+ if (_observations.hasSatellite(sat))
+ {
+ if (sats_status[i].vs)
+ {
+ output.prange_residuals.emplace(sat,sats_status[i].resp[0]);
+ output.used_sats.insert(sat);
+ }
+ else
+ output.discarded_sats.insert(sat);
+ }
+ }
+
+ return output;
+}
+
+// ComputePosOutput computePosOwn(const GnssUtils::Observations & _observations,
+// GnssUtils::Navigation & _navigation,
+// const prcopt_t & prcopt)
+// {
+
+// // Remove duplicated satellites
+// uniqnav(&(_navigation.getNavigation()));
+
+// // Define error msg
+// char msg[128] = "";
+
+// GnssUtils::ComputePosOutput output;
+// sol_t sol;
+// sol = {{0}};
+
+// output.pos_stat = pntposOwn(_observations.data(), _observations.size(),
+// &(_navigation.getNavigation()),
+// &prcopt, &sol, NULL, NULL, msg);
+
+// if (output.pos_stat == 0)
+// {
+// std::cout << "Bad computing: " << msg << "\n";
+// }
+
+// output.time = sol.time.time;
+// output.sec = sol.time.sec;
+// output.pos = Eigen::Vector3d(sol.rr);
+// // std::cout << "Compute pos: " << output.pos.transpose() << "\n";
+// output.vel = Eigen::Vector3d(&sol.rr[3]);
+// output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5],
+// sol.qr[3], sol.qr[1], sol.qr[4],
+// sol.qr[5], sol.qr[3], sol.qr[2];
+
+// // XXX: segmentation fault here.
+// // if (sol.dtr != NULL)
+// // {
+// // output.rcv_bias << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5];
+// // }
+
+// output.type = sol.type;
+// output.stat = sol.stat;
+// output.ns = sol.ns;
+// output.age = sol.age;
+// output.ratio = sol.ratio;
+// output.lat_lon = ecefToLatLonAlt(output.pos);
+
+// return output;
+// }
+
+/* single-point positioning COPIED FROM RTKLIB ------------------------------
+ * compute receiver position, velocity, clock bias by single-point positioning
+ * with pseudorange and doppler observables
+ * args : obsd_t *obs I observation data
+ * int n I number of observation data
+ * nav_t *nav I navigation data
+ * prcopt_t *opt I processing options
+ * sol_t *sol IO solution
+ * double *azel IO azimuth/elevation angle (rad) (NULL: no output)
+ * ssat_t *ssat IO satellite status (NULL: no output)
+ * char *msg O error message for error exit
+ * return : status(1:ok,0:error)
+ * notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and
+ * receiver bias are negligible (only involving glonass-gps time offset
+ * and receiver bias)
+ *-----------------------------------------------------------------------------*/
+// int pntposOwn(const obsd_t *obs, int n, const nav_t *nav,
+// const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
+// char *msg)
+// {
+// prcopt_t opt_=*opt;
+// double *rs,*dts,*var,*azel_,*resp;
+// int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];
+
+// sol->stat=SOLQ_NONE;
+
+// if (n<=0) {strcpy(msg,"no observation data"); return 0;}
+
+// trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n);
+
+// sol->time=obs[0].time; msg[0]='\0';
+
+// rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);
+
+// if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
+// #if 0
+// opt_.sateph =EPHOPT_BRDC;
+// #endif
+// opt_.ionoopt=IONOOPT_BRDC;
+// opt_.tropopt=TROPOPT_SAAS;
+// }
+// /* satellite positons, velocities and clocks */
+// satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh);
+
+// /* estimate receiver position with pseudorange */
+// stat=estposOwn(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
+
+// /* raim fde */
+// if (!stat&&n>=6&&opt->posopt[4]) {
+// stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
+// }
+// /* estimate receiver velocity with doppler */
+// //if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);
+
+// if (azel) {
+// for (i=0;i<n*2;i++) azel[i]=azel_[i];
+// }
+// if (ssat) {
+// for (i=0;i<MAXSAT;i++) {
+// ssat[i].vs=0;
+// ssat[i].azel[0]=ssat[i].azel[1]=0.0;
+// ssat[i].resp[0]=ssat[i].resc[0]=0.0;
+// ssat[i].snr[0]=0;
+// }
+// for (i=0;i<n;i++) {
+// ssat[obs[i].sat-1].azel[0]=azel_[ i*2];
+// ssat[obs[i].sat-1].azel[1]=azel_[1+i*2];
+// ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0];
+// if (!vsat[i]) continue;
+// ssat[obs[i].sat-1].vs=1;
+// ssat[obs[i].sat-1].resp[0]=resp[i];
+// }
+// }
+// free(rs); free(dts); free(var); free(azel_); free(resp);
+// return stat;
+// }
+
+int estposOwn(const obsd_t* obs,
+ int n,
+ const double* rs,
+ const double* dts,
+ const double* vare,
+ const int* svh,
+ const nav_t* nav,
+ const prcopt_t* opt,
+ sol_t* sol,
+ double* azel,
+ int* vsat,
+ double* resp,
+ char* msg)
+{
+ // double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
+ // int i,j,k,info,stat,nv,ns;
+ //
+ // trace(3,"estpos : n=%d\n",n);
+ //
+ // v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
+ //
+ // for (i=0;i<3;i++) x[i]=sol->rr[i];
+ //
+ // for (i=0;i<MAXITR;i++) {
+ //
+ // /* pseudorange residuals */
+ // nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
+ // &ns);
+ //
+ // if (nv<NX) {
+ // sprintf(msg,"lack of valid sats ns=%d",nv);
+ // break;
+ // }
+ // /* weight by variance */
+ // for (j=0;j<nv;j++) {
+ // sig=sqrt(var[j]);
+ // v[j]/=sig;
+ // for (k=0;k<NX;k++) H[k+j*NX]/=sig;
+ // }
+ // /* least square estimation */
+ // if ((info=lsq(H,v,NX,nv,dx,Q))) {
+ // sprintf(msg,"lsq error info=%d",info);
+ // break;
+ // }
+ // for (j=0;j<NX;j++) x[j]+=dx[j];
+ //
+ // if (norm(dx,NX)<1E-4) {
+ // sol->type=0;
+ // sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
+ // sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
+ // sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
+ // sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
+ // sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
+ // for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
+ // for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
+ // sol->qr[3]=(float)Q[1]; /* cov xy */
+ // sol->qr[4]=(float)Q[2+NX]; /* cov yz */
+ // sol->qr[5]=(float)Q[2]; /* cov zx */
+ // sol->ns=(unsigned char)ns;
+ // sol->age=sol->ratio=0.0;
+ //
+ // /* validate solution */
+ // if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
+ // sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
+ // }
+ // free(v); free(H); free(var);
+ //
+ // return stat;
+ // }
+ // }
+ // if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
+ //
+ // free(v); free(H); free(var);
+
+ return 0;
+}
+} // namespace GnssUtils
diff --git a/src/utils/satellite.cpp b/src/utils/satellite.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eff1d47d263f8e13ff6b4c8787837c5f8e4b2fcc
--- /dev/null
+++ b/src/utils/satellite.cpp
@@ -0,0 +1,146 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * sat_position.cpp
+ *
+ * Created on: April 3, 2020
+ * Author: Joan Vallvé, Pep MartÃ-Saumell
+ */
+
+#include "gnss_utils/utils/satellite.h"
+
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+#include "gnss_utils/utils/transformations.h"
+#include "gnss_utils/gnss_utils.h"
+
+namespace GnssUtils
+{
+
+Eigen::Vector2d computeAzel(const Eigen::Vector3d& sat_ecef, const Eigen::Vector3d& receiver_ecef)
+{
+ // receiver position (geo)
+ Eigen::Vector3d receiver_geo;
+ ecef2pos(receiver_ecef.data(), receiver_geo.data());
+
+ // receiver-sat unit vector (ecef)
+ Eigen::Vector3d e_ecef = (sat_ecef - receiver_ecef).normalized();
+
+ // receiver-sat unit vector (enu)
+ Eigen::Vector3d e_enu;
+ ecef2enu(receiver_geo.data(), // geodetic position {lat,lon} (rad)
+ e_ecef.data(), // vector in ecef coordinate {x,y,z}
+ e_enu.data()); // vector in local tangental coordinate {e,n,u}
+
+ Eigen::Vector2d azel;
+ // azimuth
+ azel(0) = atan2(e_enu(0), e_enu(1));
+
+ // elevation
+ azel(1) = asin(e_enu(2));
+
+ return azel;
+}
+
+Eigen::Vector2d computeAzel(const Satellite& sat, const Eigen::Vector3d& receiver_ecef)
+{
+ return computeAzel(sat.pos, receiver_ecef);
+ /*// compute receiver-to-satellilte unit vector (ecef)
+ Eigen::Vector3d e;
+ geodist(sat.pos.data(), receiver_ecef.data(), e.data());
+
+ // compute receiver position in geodetic
+ Eigen::Vector3d receiver_geo;
+ ecef2pos(receiver_ecef.data(), receiver_geo.data());
+
+ // compute azimut and elevation
+ Eigen::Vector2d azel;
+ satazel(receiver_geo.data(), e.data(), azel.data());
+
+ return azel;*/
+}
+
+Azels computeAzels(const Satellites& sats, const Eigen::Vector3d& receiver_ecef)
+{
+ Azels azels;
+ for (auto sat : sats)
+ azels.emplace(sat.first, computeAzel(sat.second, receiver_ecef));
+ return azels;
+}
+
+Satellites computeSatellites(const Observations& obs,
+ const Navigation& nav,
+ const int& eph_opt)
+{
+ double rs[6 * obs.size()], dts[2 * obs.size()], var[obs.size()];
+ int svh[obs.size()];
+
+ // std::cout << "computing position of sats: ";
+ // for (auto&& obs_ref : obs.getObservations())
+ // std::cout << (int)obs_ref.sat << " ";
+ // std::cout << std::endl;
+
+ // compute positions
+ if (not obs.getObservations().empty())
+ satposs(obs.getObservations().front().time,
+ obs.data(),
+ obs.size(),
+ &nav.getNavigation(),
+ eph_opt,
+ rs,
+ dts,
+ var,
+ svh);
+
+ // fill Satellites
+ Satellites sats;
+ //std::cout << "computeSatellites: filling satellites: \n";
+ for (int i = 0; i < obs.size(); i++)
+ {
+ auto sat_pair = sats.emplace(obs.getObservationByIdx(i).sat, // Key
+ Satellite({satsys(obs.getObservationByIdx(i).sat,NULL),
+ obs.getObservationByIdx(i).sat, // Constructor...
+ (Eigen::Vector3d() << rs[6*i],rs[6*i+1],rs[6*i+2]).finished(),
+ (Eigen::Vector3d() << rs[6*i+3], rs[6*i+4], rs[6*i+5]).finished() ,
+ var[i],
+ dts[2*i],
+ dts[2*i+1],
+ svh[i]}));
+ /*//DEBUG
+ if (sat_pair.first->second.pos == Eigen::Vector3d::Zero())
+ std::cout << "\tsat: " << sat_pair.first->second.sat << " ephemeris not available" << std::endl;
+ else
+ {
+ std::cout << "\tsat: " << sat_pair.first->second.sat << std::endl
+ << "\t\tpos: " << sat_pair.first->second.pos.transpose() << std::endl
+ << "\t\tvel: " << sat_pair.first->second.vel.transpose() << std::endl
+ << "\t\tvar: " << sat_pair.first->second.var << std::endl
+ << "\t\tclock bias: " << sat_pair.first->second.clock_bias << std::endl
+ << "\t\tclock drift: " << sat_pair.first->second.clock_drift << std::endl
+ << "\t\tsvh: " << sat_pair.first->second.svh << std::endl;
+ }//*/
+ }
+
+ return sats;
+}
+
+} // namespace GnssUtils
diff --git a/src/utils/transformations.cpp b/src/utils/transformations.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d1850e0145212d883d0bf3649a3c61436444ce01
--- /dev/null
+++ b/src/utils/transformations.cpp
@@ -0,0 +1,173 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gnss_utils/utils/transformations.h"
+
+using namespace GnssUtils;
+
+namespace GnssUtils
+{
+
+Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef)
+{
+ Eigen::Vector3d pos;
+ ecef2pos(_ecef.data(), pos.data());
+
+ return pos;
+}
+
+Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon)
+{
+ Eigen::Vector3d ecef;
+ pos2ecef(_latlon.data(), ecef.data());
+
+ return ecef;
+}
+
+Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d& _cov_ecef)
+{
+ Eigen::Matrix3d cov_enu;
+
+ /* RTKLIB transform covariance to local tangental coordinate --------------------------
+ * transform ecef covariance to local tangental coordinate
+ * args : double *pos I geodetic position {lat,lon} (rad)
+ * double *P I covariance in ecef coordinate
+ * double *Q O covariance in local tangental coordinate
+ * return : none
+ *-----------------------------------------------------------------------------*/
+ // extern void covenu(const double *pos, const double *P, double *Q);
+ covenu(_latlon.data(), _cov_ecef.data(), cov_enu.data());
+
+ return cov_enu;
+}
+
+Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d& _cov_enu)
+{
+ Eigen::Matrix3d cov_ecef;
+
+ /* RTKLIB transform local enu coordinate covariance to xyz-ecef -----------------------
+ * transform local enu covariance to xyz-ecef coordinate
+ * args : double *pos I geodetic position {lat,lon} (rad)
+ * double *Q I covariance in local enu coordinate
+ * double *P O covariance in xyz-ecef coordinate
+ * return : none
+ *-----------------------------------------------------------------------------*/
+ // extern void covecef(const double *pos, const double *Q, double *P)
+ covecef(_latlon.data(), _cov_enu.data(), cov_ecef.data());
+
+ return cov_ecef;
+}
+
+void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_ECEF)
+{
+ // Convert ECEF coordinates to geodetic coordinates.
+ // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
+ // to geodetic coordinates," IEEE Transactions on Aerospace and
+ // Electronic Systems, vol. 30, pp. 957-961, 1994.
+
+ // double r = std::sqrt(_t_ECEF_ENU(0) * _t_ECEF_ENU(0) + _t_ECEF_ENU(1) * _t_ECEF_ENU(1));
+ // double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
+ // double F = 54 * kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) * _t_ECEF_ENU(2);
+ // double G = r * r + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) - kFirstEccentricitySquared
+ // * Esq; double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3); double S =
+ // cbrt(1 + C + sqrt(C * C + 2 * C)); double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G); double Q = sqrt(1 + 2 *
+ // kFirstEccentricitySquared * kFirstEccentricitySquared * P); double r_0 = -(P * kFirstEccentricitySquared * r) /
+ // (1 + Q)
+ // + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
+ // - P * (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P *
+ // r * r);
+ // double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2)
+ // * _t_ECEF_ENU(2)); double Z_0 = kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) / (kSemimajorAxis * V);
+ //
+ // double latitude = atan((_t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r);
+ // double longitude = atan2(_t_ECEF_ENU(1), _t_ECEF_ENU(0));
+ //
+ //
+ // double sLat = sin(latitude);
+ // double cLat = cos(latitude);
+ // double sLon = sin(longitude);
+ // double cLon = cos(longitude);
+ //
+ // R_ENU_ECEF(0,0) = -sLon;
+ // R_ENU_ECEF(0,1) = cLon;
+ // R_ENU_ECEF(0,2) = 0.0;
+ //
+ // R_ENU_ECEF(1,0) = -sLat*cLon;
+ // R_ENU_ECEF(1,1) = -sLat * sLon;
+ // R_ENU_ECEF(1,2) = cLat;
+ //
+ // R_ENU_ECEF(2,0) = cLat * cLon;
+ // R_ENU_ECEF(2,1) = cLat * sLon;
+ // R_ENU_ECEF(2,2) = sLat;
+ //
+ // t_ENU_ECEF = -R_ENU_ECEF*_t_ECEF_ENU;
+
+ Eigen::Vector3d ENU_lat_lon_alt = GnssUtils::ecefToLatLonAlt(_t_ECEF_ENU);
+
+ double sLat = sin(ENU_lat_lon_alt(0));
+ double cLat = cos(ENU_lat_lon_alt(0));
+ double sLon = sin(ENU_lat_lon_alt(1));
+ double cLon = cos(ENU_lat_lon_alt(1));
+
+ R_ENU_ECEF(0, 0) = -sLon;
+ R_ENU_ECEF(0, 1) = cLon;
+ R_ENU_ECEF(0, 2) = 0.0;
+
+ R_ENU_ECEF(1, 0) = -sLat * cLon;
+ R_ENU_ECEF(1, 1) = -sLat * sLon;
+ R_ENU_ECEF(1, 2) = cLat;
+
+ R_ENU_ECEF(2, 0) = cLat * cLon;
+ R_ENU_ECEF(2, 1) = cLat * sLon;
+ R_ENU_ECEF(2, 2) = sLat;
+
+ t_ENU_ECEF = -R_ENU_ECEF * _t_ECEF_ENU;
+}
+
+void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_ECEF)
+{
+ double sLat = sin(_ENU_latlonalt(0));
+ double cLat = cos(_ENU_latlonalt(0));
+ double sLon = sin(_ENU_latlonalt(1));
+ double cLon = cos(_ENU_latlonalt(1));
+
+ R_ENU_ECEF(0, 0) = -sLon;
+ R_ENU_ECEF(0, 1) = cLon;
+ R_ENU_ECEF(0, 2) = 0.0;
+
+ R_ENU_ECEF(1, 0) = -sLat * cLon;
+ R_ENU_ECEF(1, 1) = -sLat * sLon;
+ R_ENU_ECEF(1, 2) = cLat;
+
+ R_ENU_ECEF(2, 0) = cLat * cLon;
+ R_ENU_ECEF(2, 1) = cLat * sLon;
+ R_ENU_ECEF(2, 2) = sLat;
+
+ Eigen::Vector3d t_ECEF_ENU = latLonAltToEcef(_ENU_latlonalt);
+
+ t_ENU_ECEF = -R_ENU_ECEF * t_ECEF_ENU;
+}
+
+} // namespace GnssUtils
diff --git a/src/utils/utils.cpp b/src/utils/utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..954a53e44176259261c0c7b6be46e075b4a732c2
--- /dev/null
+++ b/src/utils/utils.cpp
@@ -0,0 +1,417 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gnss_utils/utils/utils.h"
+
+namespace GnssUtils
+{
+void print(std::string& _msg)
+{
+ std::string msg = GNSSUTILS_MSG + _msg;
+
+ std::cout << msg << "\n";
+}
+
+std::vector<Observations>
+loadObservationsFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt)
+{
+ obs_t obs;
+ obs.data = (obsd_t*)malloc(sizeof(obsd_t) * MAXSAT);
+ obs.n = 0;
+ obs.nmax = MAXSAT;
+
+ // const char* opt = "";
+ auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, &obs, NULL, NULL);
+ int n_epochs;
+ if (stat == 1)
+ n_epochs = sortobs(&obs);
+ else
+ {
+ std::cout << "Observation: couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error")
+ << stat << std::endl;
+ return std::vector<Observations>();
+ }
+
+ std::vector<Observations> obs_vector;
+ obs_vector.reserve(n_epochs);
+
+ Observations obs_epoch;
+ for (int i = 0; i < obs.n; i++)
+ {
+ obs_epoch.addObservation(obs.data[i]);
+
+ bool last_obs_epoch = false;
+ if (i < obs.n - 1)
+ {
+ double tt = timediff(obs.data[i].time, obs.data[i + 1].time);
+ if (fabs(tt) > DTTOL)
+ {
+ obs_vector.push_back(obs_epoch);
+ obs_epoch.clearObservations();
+ }
+ }
+ else
+ {
+ obs_vector.push_back(obs_epoch);
+ }
+ }
+
+ free(obs.data);
+
+ return obs_vector;
+}
+
+} // namespace GnssUtils
+
+bool operator==(const gtime_t& time1, const gtime_t& time2)
+{
+ return (difftime(time1.time, time2.time) == 0.0 && time1.sec == time2.sec);
+}
+bool operator!=(const gtime_t& time1, const gtime_t& time2)
+{
+ return not(time1 == time2);
+}
+
+bool operator==(const obsd_t& obs1, const obsd_t& obs2)
+{
+ return obs1.time == obs2.time && obs1.eventime == obs2.eventime && obs1.timevalid == obs2.timevalid &&
+ obs1.sat == obs2.sat && obs1.rcv == obs2.rcv &&
+ GnssUtils::equalArray<unsigned char>(obs1.SNR, obs2.SNR, ARRAY_SIZE(obs1.SNR), ARRAY_SIZE(obs2.SNR)) &&
+ GnssUtils::equalArray<unsigned char>(obs1.LLI, obs2.LLI, ARRAY_SIZE(obs1.LLI), ARRAY_SIZE(obs2.LLI)) &&
+ GnssUtils::equalArray<unsigned char>(obs1.code, obs2.code, ARRAY_SIZE(obs1.code), ARRAY_SIZE(obs2.code)) &&
+ GnssUtils::equalArray<unsigned char>(obs1.qualL, obs2.qualL, ARRAY_SIZE(obs1.qualL), ARRAY_SIZE(obs2.qualL)) &&
+ GnssUtils::equalArray<unsigned char>(obs1.qualP, obs2.qualP, ARRAY_SIZE(obs1.qualP), ARRAY_SIZE(obs2.qualP)) &&
+ obs1.freq == obs2.freq &&
+ GnssUtils::equalArray<double>(obs1.L, obs2.L, ARRAY_SIZE(obs1.L), ARRAY_SIZE(obs2.L)) &&
+ GnssUtils::equalArray<double>(obs1.P, obs2.P, ARRAY_SIZE(obs1.P), ARRAY_SIZE(obs2.P)) &&
+ GnssUtils::equalArray<float>(obs1.D, obs2.D, ARRAY_SIZE(obs1.D), ARRAY_SIZE(obs2.D));
+}
+bool operator!=(const obsd_t& obs1, const obsd_t& obs2)
+{
+ return not(obs1 == obs2);
+}
+
+bool operator==(const obs_t& obs1, const obs_t& obs2)
+{
+ if (obs1.n != obs2.n)
+ return false;
+ for (int i = 0; i < obs1.n; ++i)
+ {
+ if (obs1.data[i] != obs2.data[i])
+ return false;
+ }
+
+ return obs1.nmax == obs2.nmax && obs1.flag == obs2.flag && obs1.rcvcount == obs2.rcvcount &&
+ obs1.tmcount == obs2.tmcount;
+}
+bool operator!=(const obs_t& obs1, const obs_t& obs2)
+{
+ return not(obs1 == obs2);
+}
+
+bool operator==(const eph_t& eph1, const eph_t& eph2)
+{
+ return eph1.sat == eph2.sat && eph1.iode == eph2.iode && eph1.iodc == eph2.iodc && eph1.sva == eph2.sva &&
+ eph1.svh == eph2.svh && eph1.week == eph2.week && eph1.code == eph2.code && eph1.flag == eph2.flag &&
+ eph1.toe == eph2.toe && eph1.toc == eph2.toc && eph1.ttr == eph2.ttr && eph1.A == eph2.A && eph1.e == eph2.e &&
+ eph1.i0 == eph2.i0 && eph1.OMG0 == eph2.OMG0 && eph1.omg == eph2.omg && eph1.M0 == eph2.M0 &&
+ eph1.deln == eph2.deln && eph1.OMGd == eph2.OMGd && eph1.idot == eph2.idot && eph1.crc == eph2.crc &&
+ eph1.crs == eph2.crs && eph1.cuc == eph2.cuc && eph1.cus == eph2.cus && eph1.cic == eph2.cic &&
+ eph1.cis == eph2.cis && eph1.toes == eph2.toes && eph1.fit == eph2.fit && eph1.f0 == eph2.f0 &&
+ eph1.f1 == eph2.f1 && eph1.f2 == eph2.f2 &&
+ GnssUtils::equalArray<double>(eph1.tgd, eph2.tgd, ARRAY_SIZE(eph1.tgd), ARRAY_SIZE(eph2.tgd)) &&
+ eph1.Adot == eph2.Adot && eph1.ndot == eph2.ndot;
+}
+bool operator!=(const eph_t& eph1, const eph_t& eph2)
+{
+ return not(eph1 == eph2);
+}
+
+bool operator==(const geph_t& geph1, const geph_t& geph2)
+{
+ return geph1.sat == geph2.sat && geph1.iode == geph2.iode && geph1.frq == geph2.frq && geph1.svh == geph2.svh &&
+ geph1.sva == geph2.sva && geph1.age == geph2.age && geph1.toe == geph2.toe && geph1.tof == geph2.tof &&
+ GnssUtils::equalArray<double>(geph1.pos, geph2.pos, ARRAY_SIZE(geph1.pos), ARRAY_SIZE(geph2.pos)) &&
+ GnssUtils::equalArray<double>(geph1.vel, geph2.vel, ARRAY_SIZE(geph1.vel), ARRAY_SIZE(geph2.vel)) &&
+ GnssUtils::equalArray<double>(geph1.acc, geph2.acc, ARRAY_SIZE(geph1.acc), ARRAY_SIZE(geph2.acc)) &&
+ geph1.taun == geph2.taun && geph1.gamn == geph2.gamn && geph1.dtaun == geph2.dtaun;
+}
+bool operator!=(const geph_t& geph1, const geph_t& geph2)
+{
+ return not(geph1 == geph2);
+}
+
+bool operator==(const seph_t& seph1, const seph_t& seph2)
+{
+ return seph1.sat == seph2.sat && seph1.t0 == seph2.t0 && seph1.tof == seph2.tof && seph1.sva == seph2.sva &&
+ seph1.svh == seph2.svh &&
+ GnssUtils::equalArray<double>(seph1.pos, seph2.pos, ARRAY_SIZE(seph1.pos), ARRAY_SIZE(seph2.pos)) &&
+ GnssUtils::equalArray<double>(seph1.vel, seph2.vel, ARRAY_SIZE(seph1.vel), ARRAY_SIZE(seph2.vel)) &&
+ GnssUtils::equalArray<double>(seph1.acc, seph2.acc, ARRAY_SIZE(seph1.acc), ARRAY_SIZE(seph2.acc)) &&
+ seph1.af0 == seph2.af0 && seph1.af1 == seph2.af1;
+}
+bool operator!=(const seph_t& seph1, const seph_t& seph2)
+{
+ return not(seph1 == seph2);
+}
+
+bool operator==(const peph_t& peph1, const peph_t& peph2)
+{
+ return peph1.time == peph2.time && peph1.index == peph2.index &&
+ GnssUtils::equalArray2d<double>(&peph1.pos[0][0],
+ &peph2.pos[0][0],
+ ARRAY2D_NROWS(peph1.pos),
+ ARRAY2D_NCOLS(peph1.pos),
+ ARRAY2D_NROWS(peph2.pos),
+ ARRAY2D_NCOLS(peph2.pos)) &&
+ GnssUtils::equalArray2d<float>(&peph1.std[0][0],
+ &peph2.std[0][0],
+ ARRAY2D_NROWS(peph1.std),
+ ARRAY2D_NCOLS(peph1.std),
+ ARRAY2D_NROWS(peph2.std),
+ ARRAY2D_NCOLS(peph2.std)) &&
+ GnssUtils::equalArray2d<double>(&peph1.vel[0][0],
+ &peph2.vel[0][0],
+ ARRAY2D_NROWS(peph1.vel),
+ ARRAY2D_NCOLS(peph1.vel),
+ ARRAY2D_NROWS(peph2.vel),
+ ARRAY2D_NCOLS(peph2.vel)) &&
+ GnssUtils::equalArray2d<float>(&peph1.vst[0][0],
+ &peph2.vst[0][0],
+ ARRAY2D_NROWS(peph1.vst),
+ ARRAY2D_NCOLS(peph1.vst),
+ ARRAY2D_NROWS(peph2.vst),
+ ARRAY2D_NCOLS(peph2.vst)) &&
+ GnssUtils::equalArray2d<float>(&peph1.cov[0][0],
+ &peph2.cov[0][0],
+ ARRAY2D_NROWS(peph1.cov),
+ ARRAY2D_NCOLS(peph1.cov),
+ ARRAY2D_NROWS(peph2.cov),
+ ARRAY2D_NCOLS(peph2.cov)) &&
+ GnssUtils::equalArray2d<float>(&peph1.vco[0][0],
+ &peph2.vco[0][0],
+ ARRAY2D_NROWS(peph1.vco),
+ ARRAY2D_NCOLS(peph1.vco),
+ ARRAY2D_NROWS(peph2.vco),
+ ARRAY2D_NCOLS(peph2.vco));
+}
+bool operator!=(const peph_t& peph1, const peph_t& peph2)
+{
+ return not(peph1 == peph2);
+}
+
+bool operator==(const pclk_t& pclk1, const pclk_t& pclk2)
+{
+ return pclk1.time == pclk2.time && pclk1.index == pclk2.index &&
+ GnssUtils::equalArray2d<double>(&pclk1.clk[0][0],
+ &pclk2.clk[0][0],
+ ARRAY2D_NROWS(pclk1.clk),
+ ARRAY2D_NCOLS(pclk1.clk),
+ ARRAY2D_NROWS(pclk2.clk),
+ ARRAY2D_NCOLS(pclk2.clk)) &&
+ GnssUtils::equalArray2d<float>(&pclk1.std[0][0],
+ &pclk2.std[0][0],
+ ARRAY2D_NROWS(pclk1.std),
+ ARRAY2D_NCOLS(pclk1.std),
+ ARRAY2D_NROWS(pclk2.std),
+ ARRAY2D_NCOLS(pclk2.std));
+}
+bool operator!=(const pclk_t& pclk1, const pclk_t& pclk2)
+{
+ return not(pclk1 == pclk2);
+}
+
+bool operator==(const alm_t& alm1, const alm_t& alm2)
+{
+ return alm1.sat == alm2.sat && alm1.svh == alm2.svh && alm1.svconf == alm2.svconf && alm1.week == alm2.week &&
+ alm1.toa == alm2.toa && alm1.A == alm2.A && alm1.e == alm2.e && alm1.i0 == alm2.i0 && alm1.OMG0 == alm2.OMG0 &&
+ alm1.omg == alm2.omg && alm1.M0 == alm2.M0 && alm1.OMGd == alm2.OMGd && alm1.toas == alm2.toas &&
+ alm1.f0 == alm2.f0 && alm1.f1 == alm2.f1;
+}
+bool operator!=(const alm_t& alm1, const alm_t& alm2)
+{
+ return not(alm1 == alm2);
+}
+
+bool operator==(const tec_t& tec1, const tec_t& tec2)
+{
+ return tec1.time == tec2.time &&
+ GnssUtils::equalArray<int>(tec1.ndata, tec2.ndata, ARRAY_SIZE(tec1.ndata), ARRAY_SIZE(tec2.ndata)) &&
+ tec1.rb == tec2.rb &&
+ GnssUtils::equalArray<double>(tec1.lats, tec2.lats, ARRAY_SIZE(tec1.lats), ARRAY_SIZE(tec2.lats)) &&
+ GnssUtils::equalArray<double>(tec1.lons, tec2.lons, ARRAY_SIZE(tec1.lons), ARRAY_SIZE(tec2.lons)) &&
+ GnssUtils::equalArray<double>(tec1.hgts, tec2.hgts, ARRAY_SIZE(tec1.hgts), ARRAY_SIZE(tec2.hgts));
+ // TODO: *data and *rms
+}
+bool operator!=(const tec_t& tec1, const tec_t& tec2)
+{
+ return not(tec1 == tec2);
+}
+
+bool operator==(const fcbd_t& fcbd1, const fcbd_t& fcbd2)
+{
+ return fcbd1.ts == fcbd2.ts && fcbd1.te == fcbd2.te &&
+ GnssUtils::equalArray2d<double>(&fcbd1.bias[0][0],
+ &fcbd2.bias[0][0],
+ ARRAY2D_NROWS(fcbd1.bias),
+ ARRAY2D_NCOLS(fcbd1.bias),
+ ARRAY2D_NROWS(fcbd2.bias),
+ ARRAY2D_NCOLS(fcbd2.bias)) &&
+ GnssUtils::equalArray2d<double>(&fcbd1.std[0][0],
+ &fcbd2.std[0][0],
+ ARRAY2D_NROWS(fcbd1.std),
+ ARRAY2D_NCOLS(fcbd1.std),
+ ARRAY2D_NROWS(fcbd2.std),
+ ARRAY2D_NCOLS(fcbd2.std));
+}
+bool operator!=(const fcbd_t& fcbd1, const fcbd_t& fcbd2)
+{
+ return not(fcbd1 == fcbd2);
+}
+
+bool operator==(const erp_t& erp1, const erp_t& erp2)
+{
+ // TODO
+ return true;
+}
+bool operator!=(const erp_t& erp1, const erp_t& erp2)
+{
+ return not(erp1 == erp2);
+}
+
+bool operator==(const nav_t& nav1, const nav_t& nav2)
+{
+ if (nav1.n != nav2.n)
+ return false;
+ for (int i = 0; i < nav1.n; ++i)
+ {
+ if (nav1.eph[i] != nav2.eph[i])
+ return false;
+ }
+
+ if (nav1.ng != nav2.ng)
+ return false;
+ for (int i = 0; i < nav1.ng; ++i)
+ {
+ if (nav1.geph[i] != nav2.geph[i])
+ return false;
+ }
+
+ if (nav1.ns != nav2.ns)
+ return false;
+ for (int i = 0; i < nav1.ns; ++i)
+ {
+ if (nav1.seph[i] != nav2.seph[i])
+ return false;
+ }
+
+ if (nav1.ne != nav2.ne)
+ return false;
+ for (int i = 0; i < nav1.ne; ++i)
+ {
+ if (nav1.peph[i] != nav2.peph[i])
+ return false;
+ }
+
+ if (nav1.nc != nav2.nc)
+ return false;
+ for (int i = 0; i < nav1.nc; ++i)
+ {
+ if (nav1.pclk[i] != nav2.pclk[i])
+ return false;
+ }
+
+ if (nav1.na != nav2.na)
+ return false;
+ for (int i = 0; i < nav1.na; ++i)
+ {
+ if (nav1.alm[i] != nav2.alm[i])
+ return false;
+ }
+
+ if (nav1.nt != nav2.nt)
+ return false;
+ for (int i = 0; i < nav1.nt; ++i)
+ {
+ if (nav1.tec[i] != nav2.tec[i])
+ return false;
+ }
+
+ if (nav1.nf != nav2.nf)
+ return false;
+ for (int i = 0; i < nav1.nf; ++i)
+ {
+ if (nav1.fcb[i] != nav2.fcb[i])
+ return false;
+ }
+
+ // erp to do
+
+ return nav1.nmax == nav2.nmax && nav1.ngmax == nav2.ngmax && nav1.nsmax == nav2.nsmax && nav1.nemax == nav2.nemax &&
+ nav1.ncmax == nav2.ncmax && nav1.namax == nav2.namax && nav1.ntmax == nav2.ntmax && nav1.nfmax == nav2.nfmax &&
+ nav1.erp == nav2.erp &&
+ GnssUtils::equalArray<double>(
+ nav1.utc_gps, nav2.utc_gps, ARRAY_SIZE(nav1.utc_gps), ARRAY_SIZE(nav2.utc_gps)) &&
+ GnssUtils::equalArray<double>(
+ nav1.utc_glo, nav2.utc_glo, ARRAY_SIZE(nav1.utc_glo), ARRAY_SIZE(nav2.utc_glo)) &&
+ GnssUtils::equalArray<double>(
+ nav1.utc_gal, nav2.utc_gal, ARRAY_SIZE(nav1.utc_gal), ARRAY_SIZE(nav2.utc_gal)) &&
+ GnssUtils::equalArray<double>(
+ nav1.utc_qzs, nav2.utc_qzs, ARRAY_SIZE(nav1.utc_qzs), ARRAY_SIZE(nav2.utc_qzs)) &&
+ GnssUtils::equalArray<double>(
+ nav1.utc_cmp, nav2.utc_cmp, ARRAY_SIZE(nav1.utc_cmp), ARRAY_SIZE(nav2.utc_cmp)) &&
+ GnssUtils::equalArray<double>(
+ nav1.utc_irn, nav2.utc_irn, ARRAY_SIZE(nav1.utc_irn), ARRAY_SIZE(nav2.utc_irn)) &&
+ GnssUtils::equalArray<double>(
+ nav1.utc_sbs, nav2.utc_sbs, ARRAY_SIZE(nav1.utc_sbs), ARRAY_SIZE(nav2.utc_sbs)) &&
+ GnssUtils::equalArray<double>(
+ nav1.ion_gps, nav2.ion_gps, ARRAY_SIZE(nav1.ion_gps), ARRAY_SIZE(nav2.ion_gps)) &&
+ GnssUtils::equalArray<double>(
+ nav1.ion_gal, nav2.ion_gal, ARRAY_SIZE(nav1.ion_gal), ARRAY_SIZE(nav2.ion_gal)) &&
+ GnssUtils::equalArray<double>(
+ nav1.ion_qzs, nav2.ion_qzs, ARRAY_SIZE(nav1.ion_qzs), ARRAY_SIZE(nav2.ion_qzs)) &&
+ GnssUtils::equalArray<double>(
+ nav1.ion_cmp, nav2.ion_cmp, ARRAY_SIZE(nav1.ion_cmp), ARRAY_SIZE(nav2.ion_cmp)) &&
+ GnssUtils::equalArray<double>(
+ nav1.ion_irn, nav2.ion_irn, ARRAY_SIZE(nav1.ion_irn), ARRAY_SIZE(nav2.ion_irn)) &&
+ nav1.leaps == nav2.leaps &&
+ GnssUtils::equalArray2d<double>(&nav1.lam[0][0],
+ &nav2.lam[0][0],
+ ARRAY2D_NROWS(nav1.lam),
+ ARRAY2D_NCOLS(nav1.lam),
+ ARRAY2D_NROWS(nav2.lam),
+ ARRAY2D_NCOLS(nav2.lam)) &&
+ GnssUtils::equalArray2d<double>(&nav1.cbias[0][0],
+ &nav2.cbias[0][0],
+ ARRAY2D_NROWS(nav1.cbias),
+ ARRAY2D_NCOLS(nav1.cbias),
+ ARRAY2D_NROWS(nav2.cbias),
+ ARRAY2D_NCOLS(nav2.cbias)) &&
+ GnssUtils::equalArray3d<double>(&nav1.rbias[0][0][0], &nav2.rbias[0][0][0], MAXRCV, 2, 3, MAXRCV, 2, 3) &&
+ GnssUtils::equalArray<double>(nav1.wlbias, nav2.wlbias, ARRAY_SIZE(nav1.wlbias), ARRAY_SIZE(nav2.wlbias)) &&
+ GnssUtils::equalArray<double>(
+ nav1.glo_cpbias, nav2.glo_cpbias, ARRAY_SIZE(nav1.glo_cpbias), ARRAY_SIZE(nav2.glo_cpbias)) &&
+ GnssUtils::equalArray<char>(nav1.glo_fcn, nav2.glo_fcn, ARRAY_SIZE(nav1.glo_fcn), ARRAY_SIZE(nav2.glo_fcn));
+}
+bool operator!=(const nav_t& nav1, const nav_t& nav2)
+{
+ return not(nav1 == nav2);
+}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c180b0041c8d6001ce578b5c25b9813061259dec
--- /dev/null
+++ b/test/CMakeLists.txt
@@ -0,0 +1,33 @@
+# Retrieve googletest from github & compile
+add_subdirectory(gtest)
+
+# Include gtest directory.
+include_directories(${GTEST_INCLUDE_DIRS})
+
+############# USE THIS TEST AS AN EXAMPLE ####################
+# #
+# Create a specific test executable for gtest_example #
+# gnss_utils_add_gtest(gtest_example gtest_example.cpp) #
+# target_link_libraries(gtest_example ${PROJECT_NAME}) #
+# #
+##############################################################
+
+# Navigation test
+gnss_utils_add_gtest(gtest_navigation gtest_navigation.cpp)
+target_link_libraries(gtest_navigation libgtest ${PROJECT_NAME})
+
+# Observations test
+gnss_utils_add_gtest(gtest_observations gtest_observations.cpp)
+target_link_libraries(gtest_observations libgtest ${PROJECT_NAME})
+
+# TDCP test
+gnss_utils_add_gtest(gtest_tdcp gtest_tdcp.cpp)
+target_link_libraries(gtest_tdcp ${PROJECT_NAME})
+
+# Transformations test
+gnss_utils_add_gtest(gtest_transformations gtest_transformations.cpp)
+target_link_libraries(gtest_transformations ${PROJECT_NAME})
+
+# ChiSquare test
+gnss_utils_add_gtest(gtest_chisquare gtest_chisquare.cpp)
+target_link_libraries(gtest_chisquare ${PROJECT_NAME})
\ No newline at end of file
diff --git a/test/data/sample_data.nav b/test/data/sample_data.nav
new file mode 100644
index 0000000000000000000000000000000000000000..5c4c418dc876fd45e7fddca388b4cc98b0202441
--- /dev/null
+++ b/test/data/sample_data.nav
@@ -0,0 +1,406 @@
+ 3.04 N: GNSS NAV DATA M: MIXED RINEX VERSION / TYPE
+sbf2rin-13.4.3 20200205 113410 UTC PGM / RUN BY / DATE
+GPSA 9.3132E-09 -1.4901E-08 -5.9605E-08 1.1921E-07 IONOSPHERIC CORR
+GPSB 9.6256E+04 -1.4746E+05 -1.3107E+05 9.1750E+05 IONOSPHERIC CORR
+GAL 2.9250E+01 3.8281E-01 3.2959E-03 0.0000E+00 IONOSPHERIC CORR
+GPUT -9.3132257462E-10 8.881784197E-16 503808 2083 TIME SYSTEM CORR
+GAUT 9.3132257462E-10 0.000000000E+00 345600 2083 TIME SYSTEM CORR
+GAGP 1.2514647096E-09-5.329070518E-15 345600 2083 TIME SYSTEM CORR
+ 18 LEAP SECONDS
+ END OF HEADER
+G02 2019 12 12 12 00 00-3.648423589766E-04-7.503331289627E-12 0.000000000000E+00
+ 4.700000000000E+01-1.208437500000E+02 4.378039505776E-09-1.891689129337E+00
+ -6.053596735001E-06 1.953727600630E-02 9.275972843170E-06 5.153594808578E+03
+ 3.888000000000E+05 3.371387720108E-07-2.768163608668E-01 2.346932888031E-07
+ 9.575379914494E-01 1.915312500000E+02-1.691294764827E+00-7.654961717025E-09
+ 2.000083311498E-11 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00-1.769512891769E-08 4.700000000000E+01
+ 3.868920000000E+05 4.000000000000E+00
+G04 2019 12 12 12 00 00-1.993030309677E-05-5.002220859751E-12 0.000000000000E+00
+ 1.570000000000E+02-1.387500000000E+01 4.959135139313E-09-2.339868507292E+00
+ -8.419156074524E-07 1.249722088687E-03 5.329027771950E-06 5.150576126099E+03
+ 3.888000000000E+05 5.587935447693E-09 1.915372002909E+00 1.303851604462E-08
+ 9.594040575671E-01 2.769062500000E+02-1.281079249179E+00-8.132124449911E-09
+ -3.685867816904E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 4.096000000000E+03 6.300000000000E+01-4.190951585770E-09 6.690000000000E+02
+ 3.873960000000E+05 4.000000000000E+00
+G06 2019 12 12 12 00 00-1.512947492301E-04-1.114131009672E-11 0.000000000000E+00
+ 1.000000000000E+02-1.239062500000E+02 3.927663602954E-09-1.824134113525E+00
+ -6.347894668579E-06 1.658447668888E-03 9.786337614059E-06 5.153719285965E+03
+ 3.888000000000E+05-1.490116119385E-08-2.122724576342E-01 5.401670932770E-08
+ 9.781077241470E-01 2.010000000000E+02-1.200768673201E+00-7.640318249923E-09
+ 1.214336296267E-11 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00 4.656612873077E-09 1.000000000000E+02
+ 3.858180000000E+05 4.000000000000E+00
+G07 2019 12 12 12 00 00-1.683332957327E-04-8.185452315956E-12 0.000000000000E+00
+ 5.700000000000E+01 3.909375000000E+01 4.250534194668E-09-2.882882750792E+00
+ 1.888722181320E-06 1.320131728426E-02 1.172721385956E-05 5.153718780518E+03
+ 3.888000000000E+05-7.450580596924E-09 2.943673467473E+00-1.341104507446E-07
+ 9.545190186113E-01 1.482812500000E+02-2.422380694519E+00-7.540671242082E-09
+ -4.639478967207E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00-1.117587089539E-08 5.700000000000E+01
+ 3.858180000000E+05 4.000000000000E+00
+G09 2019 12 12 12 00 00-1.062308438122E-04-9.436007530894E-12 0.000000000000E+00
+ 7.800000000000E+01-1.206250000000E+01 5.168786729286E-09 4.992301981873E-01
+ -7.078051567078E-07 1.692383317277E-03 5.021691322327E-06 5.153534730911E+03
+ 3.888000000000E+05-2.793967723846E-08 1.868752621939E+00 1.490116119385E-08
+ 9.520568016730E-01 2.776875000000E+02 1.684705661839E+00-8.283559329210E-09
+ -2.914407111040E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00 1.396983861923E-09 7.800000000000E+01
+ 3.857520000000E+05 4.000000000000E+00
+G13 2019 12 12 12 00 00-2.714386209846E-05 2.387423592154E-12 0.000000000000E+00
+ 8.700000000000E+01-1.987500000000E+01 4.779841956746E-09-8.793798521920E-01
+ -1.072883605957E-06 4.130274290219E-03 5.709007382393E-06 5.153656044006E+03
+ 3.888000000000E+05-6.332993507385E-08 2.009100851183E+00 9.313225746155E-08
+ 9.674158381471E-01 2.706562500000E+02 1.183260297004E+00-8.083193840326E-09
+ -5.078782980268E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00-1.117587089539E-08 8.700000000000E+01
+ 3.870420000000E+05 4.000000000000E+00
+G23 2019 12 12 12 00 00-1.500290818512E-04 2.387423592154E-12 0.000000000000E+00
+ 3.000000000000E+01-1.009375000000E+01 5.335222233421E-09-1.452196357053E+00
+ -4.973262548447E-07 1.326873130165E-02 4.552304744720E-06 5.153694892883E+03
+ 3.888000000000E+05 2.980232238770E-07 1.861752114203E+00 4.470348358154E-08
+ 9.428683609718E-01 2.778750000000E+02-2.215284453592E+00-8.261415549690E-09
+ -2.478674675321E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00-2.048909664154E-08 3.000000000000E+01
+ 3.858420000000E+05 4.000000000000E+00
+G30 2019 12 12 12 00 00-1.042019575834E-04-8.640199666843E-12 0.000000000000E+00
+ 2.500000000000E+01 4.368750000000E+01 4.663408535398E-09-2.906933186921E+00
+ 2.210959792137E-06 4.164319136180E-03 1.142919063568E-05 5.153722436905E+03
+ 3.888000000000E+05-2.793967723846E-08 2.968865819418E+00-3.725290298462E-09
+ 9.399056183160E-01 1.480312500000E+02-2.921787544453E+00-7.888185717455E-09
+ -4.778770483544E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00 3.725290298462E-09 2.500000000000E+01
+ 3.858180000000E+05 4.000000000000E+00
+R03 2019 12 12 11 15 00 6.369315087795E-06 0.000000000000E+00 3.858000000000E+05
+ 9.652274414063E+03 3.457994461060E-01 9.313225746155E-10 0.000000000000E+00
+ 1.708528710938E+04 2.202743530273E+00-1.862645149231E-09 5.000000000000E+00
+ 1.629986035156E+04-2.522974967957E+00-2.793967723846E-09 0.000000000000E+00
+R03 2019 12 12 11 45 00 6.371177732944E-06 0.000000000000E+00 3.870000000000E+05
+ 1.043946044922E+04 4.914197921753E-01 9.313225746155E-10 0.000000000000E+00
+ 2.039817236328E+04 1.453030586243E+00-1.862645149231E-09 5.000000000000E+00
+ 1.118693066406E+04-3.121288299561E+00-1.862645149231E-09 0.000000000000E+00
+R05 2019 12 12 11 15 00 4.108343273401E-05 9.094947017729E-13 3.862800000000E+05
+ 5.985222167969E+03 1.195173263550E-01-1.862645149231E-09 0.000000000000E+00
+ -1.828012353516E+04 2.295486450195E+00 1.862645149231E-09 1.000000000000E+00
+ 1.676849218750E+04 2.457766532898E+00-1.862645149231E-09 0.000000000000E+00
+R05 2019 12 12 11 45 00 4.108529537916E-05 9.094947017729E-13 3.871800000000E+05
+ 6.598400390625E+03 5.781021118164E-01-9.313225746155E-10 0.000000000000E+00
+ -1.370180371094E+04 2.745733261108E+00 9.313225746155E-10 1.000000000000E+00
+ 2.048785058594E+04 1.648019790649E+00-1.862645149231E-09 0.000000000000E+00
+R14 2019 12 12 11 15 00 4.597380757332E-05 0.000000000000E+00 3.858000000000E+05
+ 2.135816357422E+04-6.865911483765E-01 3.725290298462E-09 0.000000000000E+00
+ 1.261080761719E+04-4.694700241089E-01-0.000000000000E+00-7.000000000000E+00
+ 5.991423828125E+03 3.442845344543E+00-2.793967723846E-09 0.000000000000E+00
+R14 2019 12 12 11 45 00 4.597473889589E-05 0.000000000000E+00 3.870000000000E+05
+ 1.936734765625E+04-1.519350051880E+00 1.862645149231E-09 0.000000000000E+00
+ 1.162212011719E+04-5.869359970093E-01-9.313225746155E-10-7.000000000000E+00
+ 1.187716162109E+04 3.054467201233E+00-2.793967723846E-09 0.000000000000E+00
+R15 2019 12 12 11 15 00 1.051910221577E-04 0.000000000000E+00 3.860100000000E+05
+ 2.249286962891E+04 1.470921516418E+00 5.587935447693E-09 0.000000000000E+00
+ 4.179755859375E+03 4.950771331787E-01 9.313225746155E-10 0.000000000000E+00
+ -1.125261865234E+04 3.118181228638E+00-1.862645149231E-09 0.000000000000E+00
+R15 2019 12 12 11 45 00 1.051910221577E-04 0.000000000000E+00 3.870000000000E+05
+ 2.451276953125E+04 7.425718307495E-01 4.656612873077E-09 0.000000000000E+00
+ 4.643450683594E+03 4.951381683350E-02 0.000000000000E+00 0.000000000000E+00
+ -5.276882812500E+03 3.478320121765E+00-2.793967723846E-09 0.000000000000E+00
+R19 2019 12 12 11 15 00-6.853323429823E-05-1.818989403546E-12 3.859500000000E+05
+ 2.008951708984E+04 2.060537338257E-01 2.793967723846E-09 0.000000000000E+00
+ -1.541425244141E+04-4.377956390381E-01 2.793967723846E-09 3.000000000000E+00
+ 3.049126953125E+03-3.578741073608E+00-1.862645149231E-09 0.000000000000E+00
+R19 2019 12 12 11 45 00-6.853695958853E-05-1.818989403546E-12 3.870000000000E+05
+ 1.978871044922E+04-5.135507583618E-01 3.725290298462E-09 0.000000000000E+00
+ -1.571828076172E+04 1.333026885986E-01 1.862645149231E-09 3.000000000000E+00
+ -3.427280273438E+03-3.570507049561E+00-1.862645149231E-09 0.000000000000E+00
+R13 2019 12 12 11 45 00-3.215298056602E-05-0.000000000000E+00 3.876000000000E+05
+ 3.100402832031E+03-2.939124107361E+00-1.862645149231E-09 0.000000000000E+00
+ 1.186301953125E+04-8.695030212402E-01-9.313225746155E-10-2.000000000000E+00
+ 2.235348193359E+04 8.685455322266E-01-1.862645149231E-09 0.000000000000E+00
+R20 2019 12 12 11 45 00-3.986386582255E-04-0.000000000000E+00 3.877800000000E+05
+ 9.837636718750E+03 1.428291320801E+00-0.000000000000E+00 0.000000000000E+00
+ -1.795626220703E+04-1.534766197205E+00 1.862645149231E-09 2.000000000000E+00
+ 1.525312060547E+04-2.726587295532E+00-1.862645149231E-09 0.000000000000E+00
+E01 2019 12 12 11 10 00-7.507699192502E-04-7.986500349944E-12 0.000000000000E+00
+ 3.000000000000E+00 2.013125000000E+02 2.449744898851E-09 7.981291655576E-01
+ 9.473413228989E-06 1.731918891892E-04 9.533017873764E-06 5.440622966766E+03
+ 3.858000000000E+05 3.166496753693E-08-2.605880888987E+00-1.117587089539E-08
+ 9.855193908613E-01 1.463750000000E+02-1.069210219000E+00-5.272719629937E-09
+ 1.914365455291E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09-1.862645149231E-09
+ 3.864650000000E+05
+E01 2019 12 12 11 20 00-7.507746340707E-04-7.972289495228E-12 0.000000000000E+00
+ 4.000000000000E+00 2.002500000000E+02 2.449744898851E-09 8.736282654153E-01
+ 9.419396519661E-06 1.731686061248E-04 9.588897228241E-06 5.440622581482E+03
+ 3.864000000000E+05 3.725290298462E-08-2.605884063520E+00-1.862645149231E-09
+ 9.855195488564E-01 1.452500000000E+02-1.070324887968E+00-5.265933632987E-09
+ 1.896507568581E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09-1.862645149231E-09
+ 3.870650000000E+05
+E09 2019 12 12 11 10 00 6.223833654076E-03-1.216449163621E-11 0.000000000000E+00
+ 3.000000000000E+00 7.218750000000E+00 3.823373544569E-09-3.442271110123E-02
+ 3.892928361893E-07 2.964780433103E-04 3.403052687645E-06 5.440603132248E+03
+ 3.858000000000E+05-9.313225746155E-09 1.575307486314E+00-6.146728992462E-08
+ 9.586413959478E-01 2.625000000000E+02 2.794206052069E-01-5.815242228181E-09
+ 8.571785620706E-12 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-4.656612873077E-10-6.984919309616E-10
+ 3.869030000000E+05
+E12 2019 12 12 10 40 00 6.055364210624E-03-1.870148480521E-11 0.000000000000E+00
+ 0.000000000000E+00-2.075312500000E+02 2.603322724555E-09-2.414195903395E+00
+ -9.659677743912E-06 2.996901748702E-04 6.495043635368E-06 5.440614477158E+03
+ 3.840000000000E+05-2.421438694000E-08-5.219517721978E-01 5.774199962616E-08
+ 9.864495053986E-01 2.151250000000E+02-9.348391204646E-01-5.376652530588E-09
+ -2.257236880119E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.327134668827E-08-1.327134668827E-08
+ 3.858050000000E+05
+E19 2019 12 12 11 00 00-2.469634637237E-06 2.273736754432E-13 0.000000000000E+00
+ 2.000000000000E+00 1.103125000000E+01 3.765871149364E-09-1.685770007887E-01
+ 4.824250936508E-07 1.212444622070E-04 3.688037395477E-06 5.440600940704E+03
+ 3.852000000000E+05-5.401670932770E-08 1.580615112564E+00 5.215406417847E-08
+ 9.583897608719E-01 2.595937500000E+02 1.996512379669E+00-5.770954669140E-09
+ 1.160762636137E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09-5.820766091347E-09
+ 3.858660000000E+05
+E19 2019 12 12 11 10 00-2.469576429576E-06 2.273736754432E-13 0.000000000000E+00
+ 3.000000000000E+00 1.168750000000E+01 3.765156833895E-09-9.388288915917E-02
+ 5.066394805908E-07 1.211694907397E-04 3.693625330925E-06 5.440600774765E+03
+ 3.858000000000E+05-5.960464477539E-08 1.580611582543E+00 4.097819328308E-08
+ 9.583898398694E-01 2.594687500000E+02 1.996204499781E+00-5.764168672191E-09
+ 1.207193141583E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09-5.820766091347E-09
+ 3.864650000000E+05
+E21 2019 12 12 11 10 00-5.709171527997E-04-2.131628207280E-12 0.000000000000E+00
+ 3.000000000000E+00 2.180312500000E+02 2.333311477503E-09 1.537835498027E+00
+ 1.022964715958E-05 9.841390419751E-05 9.685754776001E-06 5.440636165619E+03
+ 3.858000000000E+05-2.980232238770E-08-2.608584029515E+00 4.842877388000E-08
+ 9.850865499746E-01 1.424062500000E+02-1.025413854454E+00-5.217717338871E-09
+ 1.871506527187E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 2.328306436539E-10
+ 3.867350000000E+05
+E21 2019 12 12 11 20 00-5.709183751605E-04-2.131628207280E-12 0.000000000000E+00
+ 4.000000000000E+00 2.213125000000E+02 2.317953694933E-09 1.607657241034E+00
+ 1.038052141666E-05 9.810912888497E-05 9.816139936447E-06 5.440637243271E+03
+ 3.864000000000E+05-4.097819328308E-08-2.608587130902E+00 4.470348358154E-08
+ 9.850867050439E-01 1.395312500000E+02-1.020851268976E+00-5.213788603794E-09
+ 1.800074980348E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 2.328306436539E-10
+ 3.870650000000E+05
+E01 2019 12 12 11 10 00-7.507689879276E-04-7.986500349944E-12 0.000000000000E+00
+ 3.000000000000E+00 2.013125000000E+02 2.449744898851E-09 7.981291655576E-01
+ 9.473413228989E-06 1.731918891892E-04 9.533017873764E-06 5.440622966766E+03
+ 3.858000000000E+05 3.166496753693E-08-2.605880888987E+00-1.117587089539E-08
+ 9.855193908613E-01 1.463750000000E+02-1.069210219000E+00-5.272719629937E-09
+ 1.914365455291E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09 0.000000000000E+00
+ 3.865400000000E+05
+E01 2019 12 12 11 20 00-7.507736445405E-04-7.972289495228E-12 0.000000000000E+00
+ 4.000000000000E+00 2.002500000000E+02 2.449744898851E-09 8.736282654153E-01
+ 9.419396519661E-06 1.731686061248E-04 9.588897228241E-06 5.440622581482E+03
+ 3.864000000000E+05 3.725290298462E-08-2.605884063520E+00-1.862645149231E-09
+ 9.855195488564E-01 1.452500000000E+02-1.070324887968E+00-5.265933632987E-09
+ 1.896507568581E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09 0.000000000000E+00
+ 3.871400000000E+05
+E12 2019 12 12 10 40 00 6.055364909116E-03-1.871569565992E-11 0.000000000000E+00
+ 0.000000000000E+00-2.075312500000E+02 2.603322724555E-09-2.414195903395E+00
+ -9.659677743912E-06 2.996901748702E-04 6.495043635368E-06 5.440614477158E+03
+ 3.840000000000E+05-2.421438694000E-08-5.219517721978E-01 5.774199962616E-08
+ 9.864495053986E-01 2.151250000000E+02-9.348391204646E-01-5.376652530588E-09
+ -2.257236880119E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.327134668827E-08 0.000000000000E+00
+ 3.858400000000E+05
+E19 2019 12 12 11 00 00-2.468528691679E-06 2.273736754432E-13 0.000000000000E+00
+ 2.000000000000E+00 1.103125000000E+01 3.765871149364E-09-1.685770007887E-01
+ 4.824250936508E-07 1.212444622070E-04 3.688037395477E-06 5.440600940704E+03
+ 3.852000000000E+05-5.401670932770E-08 1.580615112564E+00 5.215406417847E-08
+ 9.583897608719E-01 2.595937500000E+02 1.996512379669E+00-5.770954669140E-09
+ 1.160762636137E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09 0.000000000000E+00
+ 3.859400000000E+05
+E19 2019 12 12 11 10 00-2.468470484018E-06 2.273736754432E-13 0.000000000000E+00
+ 3.000000000000E+00 1.168750000000E+01 3.765156833895E-09-9.388288915917E-02
+ 5.066394805908E-07 1.211694907397E-04 3.693625330925E-06 5.440600774765E+03
+ 3.858000000000E+05-5.960464477539E-08 1.580611582543E+00 4.097819328308E-08
+ 9.583898398694E-01 2.594687500000E+02 1.996204499781E+00-5.764168672191E-09
+ 1.207193141583E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09 0.000000000000E+00
+ 3.865400000000E+05
+E21 2019 12 12 11 10 00-5.709161632694E-04-2.117417352565E-12 0.000000000000E+00
+ 3.000000000000E+00 2.180312500000E+02 2.333311477503E-09 1.537835498027E+00
+ 1.022964715958E-05 9.841390419751E-05 9.685754776001E-06 5.440636165619E+03
+ 3.858000000000E+05-2.980232238770E-08-2.608584029515E+00 4.842877388000E-08
+ 9.850865499746E-01 1.424062500000E+02-1.025413854454E+00-5.217717338871E-09
+ 1.871506527187E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 0.000000000000E+00
+ 3.867900000000E+05
+E21 2019 12 12 11 20 00-5.709173856303E-04-2.131628207280E-12 0.000000000000E+00
+ 4.000000000000E+00 2.213125000000E+02 2.317953694933E-09 1.607657241034E+00
+ 1.038052141666E-05 9.810912888497E-05 9.816139936447E-06 5.440637243271E+03
+ 3.864000000000E+05-4.097819328308E-08-2.608587130902E+00 4.470348358154E-08
+ 9.850867050439E-01 1.395312500000E+02-1.020851268976E+00-5.213788603794E-09
+ 1.800074980348E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 0.000000000000E+00
+ 3.871400000000E+05
+E04 2019 12 12 11 10 00-4.236891982146E-04-7.531752999057E-12 0.000000000000E+00
+ 3.000000000000E+00 9.781250000000E+00 3.935521073107E-09-4.009111796513E-01
+ 5.532056093216E-07 8.811207953840E-05 3.596767783165E-06 5.440604427338E+03
+ 3.858000000000E+05 9.499490261078E-08 1.582511472789E+00 3.166496753693E-08
+ 9.526713574660E-01 2.577187500000E+02 1.440283619102E+00-5.908817554540E-09
+ 7.071723137082E-11 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.355104804039E-09-5.820766091347E-09
+ 3.875670000000E+05
+E04 2019 12 12 11 10 00-4.236878594384E-04-7.531752999057E-12 0.000000000000E+00
+ 3.000000000000E+00 9.781250000000E+00 3.935521073107E-09-4.009111796513E-01
+ 5.532056093216E-07 8.811207953840E-05 3.596767783165E-06 5.440604427338E+03
+ 3.858000000000E+05 9.499490261078E-08 1.582511472789E+00 3.166496753693E-08
+ 9.526713574660E-01 2.577187500000E+02 1.440283619102E+00-5.908817554540E-09
+ 7.071723137082E-11 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.355104804039E-09 0.000000000000E+00
+ 3.875900000000E+05
+E11 2019 12 12 11 00 00 3.295102505945E-04 1.526245796413E-10 5.204170427930E-18
+ 2.000000000000E+00-2.123750000000E+02 2.665468170305E-09 2.480654590844E+00
+ -9.858980774879E-06 1.651302445680E-04 7.150694727898E-06 5.440609029770E+03
+ 3.852000000000E+05-3.352761268616E-08-5.219456059981E-01 2.048909664154E-08
+ 9.864349859366E-01 1.975937500000E+02-2.012781103955E-01-5.400939256513E-09
+ -2.103659054415E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.583248376846E-08-1.699663698673E-08
+ 3.876330000000E+05
+E21 2019 12 12 11 30 00-5.709196557291E-04-2.131628207280E-12 0.000000000000E+00
+ 5.000000000000E+00 2.240625000000E+02 2.303667385565E-09 1.677767938158E+00
+ 1.050904393196E-05 9.777839295566E-05 9.970739483833E-06 5.440638303757E+03
+ 3.870000000000E+05-5.401670932770E-08-2.608590226436E+00 3.725290298462E-08
+ 9.850868571874E-01 1.362812500000E+02-1.016577701982E+00-5.210574184187E-09
+ 1.721500278825E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 2.328306436539E-10
+ 3.876650000000E+05
+E11 2019 12 12 11 00 00 3.295124042779E-04 1.526387904960E-10 5.204170427930E-18
+ 2.000000000000E+00-2.123750000000E+02 2.665468170305E-09 2.480654590844E+00
+ -9.858980774879E-06 1.651302445680E-04 7.150694727898E-06 5.440609029770E+03
+ 3.852000000000E+05-3.352761268616E-08-5.219456059981E-01 2.048909664154E-08
+ 9.864349859366E-01 1.975937500000E+02-2.012781103955E-01-5.400939256513E-09
+ -2.103659054415E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.583248376846E-08 0.000000000000E+00
+ 3.876800000000E+05
+E01 2019 12 12 11 30 00-7.507792906836E-04-7.972289495228E-12 0.000000000000E+00
+ 5.000000000000E+00 1.994687500000E+02 2.447601952446E-09 9.486982138657E-01
+ 9.378418326378E-06 1.731649972498E-04 9.616836905479E-06 5.440622346878E+03
+ 3.870000000000E+05 3.911554813385E-08-2.605887229274E+00 7.450580596924E-09
+ 9.855197039257E-01 1.446875000000E+02-1.071010335308E+00-5.259504793772E-09
+ 1.871506527187E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09-1.862645149231E-09
+ 3.877250000000E+05
+E21 2019 12 12 11 30 00-5.709186661988E-04-2.117417352565E-12 0.000000000000E+00
+ 5.000000000000E+00 2.240625000000E+02 2.303667385565E-09 1.677767938158E+00
+ 1.050904393196E-05 9.777839295566E-05 9.970739483833E-06 5.440638303757E+03
+ 3.870000000000E+05-5.401670932770E-08-2.608590226436E+00 3.725290298462E-08
+ 9.850868571874E-01 1.362812500000E+02-1.016577701982E+00-5.210574184187E-09
+ 1.721500278825E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 0.000000000000E+00
+ 3.877400000000E+05
+E01 2019 12 12 11 30 00-7.507783011533E-04-7.972289495228E-12 0.000000000000E+00
+ 5.000000000000E+00 1.994687500000E+02 2.447601952446E-09 9.486982138657E-01
+ 9.378418326378E-06 1.731649972498E-04 9.616836905479E-06 5.440622346878E+03
+ 3.870000000000E+05 3.911554813385E-08-2.605887229274E+00 7.450580596924E-09
+ 9.855197039257E-01 1.446875000000E+02-1.071010335308E+00-5.259504793772E-09
+ 1.871506527187E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09 0.000000000000E+00
+ 3.878200000000E+05
+E36 2019 12 12 10 30 00 6.253067986108E-04-4.803268893738E-12 0.000000000000E+00
+ 1.270000000000E+02-2.396562500000E+02 2.521890761159E-09 3.060908164528E+00
+ -1.121312379837E-05 3.076605498791E-04 7.288530468941E-06 5.440609262466E+03
+ 3.834000000000E+05 2.980232238770E-08-5.264936383676E-01 5.587935447693E-08
+ 9.896539850266E-01 1.944062500000E+02-1.782833699345E+00-5.428440402046E-09
+ -2.457245211269E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 3.725290298462E-09 4.423782229424E-09
+ 3.878450000000E+05
+S36 2019 12 12 11 36 48 0.000000000000E+00 0.000000000000E+00 3.874820000000E+05
+ 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 2.530000000000E+02
+S25 2019 12 11 23 59 44 0.000000000000E+00 0.000000000000E+00 3.874840000000E+05
+ 4.053076976000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ -1.162201120000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00
+S23 2019 12 12 11 37 04 0.000000000000E+00 0.000000000000E+00 3.874940000000E+05
+ 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.460000000000E+02
+S36 2019 12 12 11 38 56 0.000000000000E+00 0.000000000000E+00 3.875460000000E+05
+ 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 2.540000000000E+02
+S23 2019 12 12 11 39 12 0.000000000000E+00 0.000000000000E+00 3.875580000000E+05
+ 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.470000000000E+02
+S23 2019 12 12 11 41 20 0.000000000000E+00 0.000000000000E+00 3.876860000000E+05
+ 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.480000000000E+02
+S36 2019 12 12 11 41 04 0.000000000000E+00 0.000000000000E+00 3.877380000000E+05
+ 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 2.550000000000E+02
+S36 2019 12 12 11 43 12 0.000000000000E+00 0.000000000000E+00 3.878020000000E+05
+ 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00
+S23 2019 12 12 11 43 28 0.000000000000E+00 0.000000000000E+00 3.878780000000E+05
+ 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.490000000000E+02
+C05 2019 12 12 11 00 00-2.244751667604E-04-5.808153957787E-11 0.000000000000E+00
+ 1.000000000000E+00-4.986718750000E+02 1.785788670980E-12-5.169766818568E-01
+ -1.630326732993E-05 6.773804780096E-04 2.061109989882E-05 6.493346771240E+03
+ 3.852000000000E+05 1.271255314350E-07-1.766797704845E-01 3.259629011154E-09
+ 1.180676987021E-01-6.241562500000E+02-1.608095995036E+00 1.192549674481E-09
+ 7.575315542299E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00-8.000000000000E-10-9.200000000000E-09
+ 3.858276000000E+05 0.000000000000E+00
+C10 2019 12 12 11 00 00-4.537283675745E-04-3.685762806072E-11 0.000000000000E+00
+ 1.000000000000E+00 9.611718750000E+02 9.743262988869E-10-3.040647920928E+00
+ 3.135204315186E-05 6.156359100714E-03 7.578637450933E-06 6.493785018921E+03
+ 3.852000000000E+05-1.676380634308E-08-2.379959881122E+00-4.004687070847E-08
+ 9.058375010021E-01-1.493750000000E+01-2.570309406835E+00-2.115088101909E-09
+ 4.782342060886E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00 5.000000000000E-09 2.400000000000E-09
+ 3.858180000000E+05 0.000000000000E+00
+C23 2019 12 12 11 00 00-8.696540025994E-04 2.136069099379E-12 0.000000000000E+00
+ 1.000000000000E+00 1.034062500000E+02 3.498002848716E-09 1.631067082891E+00
+ 5.179084837437E-06 2.014992060140E-04 1.188227906823E-05 5.282628129959E+03
+ 3.852000000000E+05 3.725290298462E-08-3.016985033742E+00 6.565824151039E-08
+ 9.546959600159E-01 1.175156250000E+02-1.126531243634E+00-6.488127399406E-09
+ -1.864363372504E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00 2.430000000000E-08 2.430000000000E-08
+ 3.858180000000E+05 1.000000000000E+00
+C30 2019 12 12 11 00 00 2.388220746070E-04 6.104450278599E-12 0.000000000000E+00
+ 1.000000000000E+00-1.393437500000E+02 3.927663602954E-09-2.732742750025E+00
+ -6.837770342827E-06 3.649030113593E-04 1.750886440277E-06 5.282614929199E+03
+ 3.852000000000E+05-3.864988684654E-08-9.521190897989E-01-5.587935447693E-09
+ 9.644793109832E-01 3.275781250000E+02-4.157902561103E-01-7.082080711374E-09
+ -2.339383158984E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00-1.040000000000E-08-1.040000000000E-08
+ 3.858180000000E+05 1.000000000000E+00
+C32 2019 12 12 11 00 00-8.566775359213E-04 2.992273095970E-12 0.000000000000E+00
+ 1.000000000000E+00 2.875000000000E+01 4.228033257413E-09 2.867304095205E+00
+ 1.456588506699E-06 1.842749770731E-04 6.427057087421E-06 5.282614164352E+03
+ 3.852000000000E+05 1.862645149231E-09 1.148173467908E+00 6.705522537231E-08
+ 9.608588315814E-01 2.297500000000E+02-1.050918258529E+00-6.857785654299E-09
+ 5.228789228631E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00-9.900000000000E-09-9.900000000000E-09
+ 3.857580000000E+05 1.000000000000E+00
+C37 2019 12 12 11 00 00-8.880557725206E-04-5.954881032721E-11 0.000000000000E+00
+ 1.000000000000E+00 1.086562500000E+02 3.521932416908E-09 8.066563687499E-01
+ 5.388632416725E-06 7.037379546091E-04 1.127412542701E-05 5.282630052567E+03
+ 3.852000000000E+05-2.142041921616E-08-3.020306267399E+00 1.536682248116E-08
+ 9.560827215058E-01 1.306718750000E+02-1.080448983277E+00-6.580988410297E-09
+ -2.328668426958E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00-1.390000000000E-08-1.390000000000E-08
+ 3.858180000000E+05 1.000000000000E+00
+C20 2019 12 12 11 00 00-4.067538538948E-04-5.008171655163E-11 0.000000000000E+00
+ 1.000000000000E+00 2.850000000000E+01 4.229461888350E-09 1.463145840983E+00
+ 1.523178070784E-06 5.460308166221E-04 6.177462637424E-06 5.282614208221E+03
+ 3.852000000000E+05 3.445893526077E-08 1.153432718382E+00-1.210719347000E-08
+ 9.622213291121E-01 2.341406250000E+02-4.480160490039E-01-6.852785446020E-09
+ 4.753769442150E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00 2.090000000000E-08 2.090000000000E-08
+ 3.875880000000E+05 1.000000000000E+00
diff --git a/test/data/sample_data.obs b/test/data/sample_data.obs
new file mode 100644
index 0000000000000000000000000000000000000000..d773d3b4d57556afcd38b64ef4167981a00de992
--- /dev/null
+++ b/test/data/sample_data.obs
@@ -0,0 +1,58 @@
+ 3.04 OBSERVATION DATA M RINEX VERSION / TYPE
+sbf2rin-13.4.3 20200205 113408 UTC PGM / RUN BY / DATE
+SEPT MARKER NAME
+Unknown MARKER NUMBER
+Unknown Unknown OBSERVER / AGENCY
+3021420 SEPT ASTERX-M2 4.4.0 REC # / TYPE / VERS
+Unknown Unknown ANT # / TYPE
+ 4789398.3686 176958.8129 4194502.0999 APPROX POSITION XYZ
+ 0.0000 0.0000 0.0000 ANTENNA: DELTA H/E/N
+G 7 X1 C1C L1C C2W L2W C2L L2L SYS / # / OBS TYPES
+E 7 X1 C1C L1C C5Q L5Q C7Q L7Q SYS / # / OBS TYPES
+S 3 X1 C1C L1C SYS / # / OBS TYPES
+R 5 X1 C1C L1C C2C L2C SYS / # / OBS TYPES
+C 5 X1 C2I L2I C7I L7I SYS / # / OBS TYPES
+SEPTENTRIO RECEIVERS OUTPUT ALIGNED CARRIER PHASES. COMMENT
+NO FURTHER PHASE SHIFT APPLIED IN THE RINEX ENCODER. COMMENT
+G L1C SYS / PHASE SHIFT
+G L2W SYS / PHASE SHIFT
+G L2L 0.00000 SYS / PHASE SHIFT
+E L1C 0.00000 SYS / PHASE SHIFT
+E L5Q 0.00000 SYS / PHASE SHIFT
+E L7Q 0.00000 SYS / PHASE SHIFT
+S L1C SYS / PHASE SHIFT
+R L1C SYS / PHASE SHIFT
+R L2C SYS / PHASE SHIFT
+C L2I SYS / PHASE SHIFT
+C L7I SYS / PHASE SHIFT
+ 0.050 INTERVAL
+ 2019 12 12 11 37 42.0000000 GPS TIME OF FIRST OBS
+ 2019 12 12 11 45 12.9000000 GPS TIME OF LAST OBS
+ 36 # OF SATELLITES
+ C1C 0.000 C2C 0.000 GLONASS COD/PHS/BIS
+ 8 R03 5 R05 1 R13 -2 R14 -7 R15 0 R19 3 R20 2 R21 4 GLONASS SLOT / FRQ #
+ END OF HEADER
+> 2019 12 12 11 37 42.0000000 0 23
+C05 16.000 40138840.659 6 209013478.40606 40138832.256 6 161622441.87406
+C10 17.000 39633104.355 5 206379985.90205 39633100.852 6 159586077.69206
+C23 18.000 22865676.301 8 119067585.90908
+C32 19.000 22153454.599 8 115358921.19508
+C37 20.000 23690566.611 7 123363010.77907
+E01 11.000 26311074.676 7 138265636.53007 26311074.470 5 103250320.19005 26311070.977 7 105943790.57907
+E09 12.000 25723493.482 5
+E12 13.000 24803095.627 5 130341141.55605 24803092.768 4 97332661.71504 24803092.407 6 99871767.93506
+E19 14.000 24540665.652 6 128962068.93606 24540664.837 5 96302837.53105 24540661.694 6 98815075.63606
+E21 15.000 25609802.251 7 134580416.73507 25609802.381 7 100498367.38007 25609799.519 7 103120055.01507
+G02 1.000 22032430.127 5
+G06 2.000 22147435.410 6 116385570.45206 22147437.095 4 90690045.24704 22147437.422 7 90690046.16907
+G07 3.000 20902411.481 8 109842911.60108 20902406.002 6 85591857.15006 20902406.930 7 85591857.16507
+G09 4.000 21908403.525 6 115129430.37406 21908399.544 1 89711233.32401 21908399.320 5
+G23 5.000 24104714.314 6 126671129.49306 24104706.816 2 98704739.67602
+G30 6.000 21404145.095 8 112479542.93308 21404142.953 6 87646388.26706 21404143.916 7 87646383.27307
+R03 7.000 21945110.617 7 117474090.63807 21945109.720 6 91368580.68606
+R14 8.000 20214975.526 8 107757316.39008 20214976.579 7 83811176.41707
+R15 9.000 22699008.378 5 121296675.96005 22699009.712 5 94341922.34705
+R19 10.000 22853592.745 4 22853590.888 4
+S23 21.000 38309228.895 7 201316353.13407
+S25 22.000 37834172.957 6 198818604.39206
+S36 23.000 37630702.258 7 197750698.01307
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3c3c48c74516488a8c4eb24c2492bd81a5edc7f9
--- /dev/null
+++ b/test/gtest/CMakeLists.txt
@@ -0,0 +1,65 @@
+cmake_minimum_required(VERSION 2.8.8)
+project(gtest_builder C CXX)
+
+# We need thread support
+#find_package(Threads REQUIRED)
+
+# Enable ExternalProject CMake module
+include(ExternalProject)
+
+set(GTEST_FORCE_SHARED_CRT ON)
+set(GTEST_DISABLE_PTHREADS OFF)
+
+# For some reason I need to disable PTHREADS
+# with g++ (Ubuntu 4.9.3-8ubuntu2~14.04) 4.9.3
+# This is a known issue for MinGW :
+# https://github.com/google/shaderc/pull/174
+#if(MINGW)
+ set(GTEST_DISABLE_PTHREADS ON)
+#endif()
+
+# Download GoogleTest
+ExternalProject_Add(googletest
+ GIT_REPOSITORY https://github.com/google/googletest.git
+ GIT_TAG v1.8.x
+ # TIMEOUT 1 # We'll try this
+ CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
+ -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
+ -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
+ -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
+ -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
+ -DBUILD_GTEST=ON
+ PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
+ # Disable install step
+ INSTALL_COMMAND ""
+ UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
+)
+
+# Get GTest source and binary directories from CMake project
+
+# Specify include dir
+ExternalProject_Get_Property(googletest source_dir)
+set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
+
+# Specify MainTest's link libraries
+ExternalProject_Get_Property(googletest binary_dir)
+set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
+
+# Create a libgtest target to be used as a dependency by test programs
+add_library(libgtest IMPORTED STATIC GLOBAL)
+add_dependencies(libgtest googletest)
+
+# Set libgtest properties
+set_target_properties(libgtest PROPERTIES
+ "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
+ "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
+)
+
+function(gnss_utils_add_gtest target)
+ add_executable(${target} ${ARGN})
+ add_dependencies(${target} libgtest)
+ target_link_libraries(${target} libgtest)
+
+ #WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/bin
+ add_test(NAME ${target} COMMAND ${target})
+endfunction()
diff --git a/test/gtest/utils_gtest.h b/test/gtest/utils_gtest.h
new file mode 100644
index 0000000000000000000000000000000000000000..c4cf21c1624b72671c79bf162bdc67da7ffe9466
--- /dev/null
+++ b/test/gtest/utils_gtest.h
@@ -0,0 +1,167 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * \file utils_gtest.h
+ * \brief Some utils for gtest
+ * \author Jeremie Deray
+ * Created on: 26/09/2016
+ * Eigen macros extension by: Joan Sola on 26/04/2017
+ */
+
+#ifndef GNSSUTILS_UTILS_GTEST_H
+#define GNSSUTILS_UTILS_GTEST_H
+
+#include <gtest/gtest.h>
+
+// Macros for testing equalities and inequalities.
+//
+// * {ASSERT|EXPECT}_EQ(expected, actual): Tests that expected == actual
+// * {ASSERT|EXPECT}_NE(v1, v2): Tests that v1 != v2
+// * {ASSERT|EXPECT}_LT(v1, v2): Tests that v1 < v2
+// * {ASSERT|EXPECT}_LE(v1, v2): Tests that v1 <= v2
+// * {ASSERT|EXPECT}_GT(v1, v2): Tests that v1 > v2
+// * {ASSERT|EXPECT}_GE(v1, v2): Tests that v1 >= v2
+//
+// C String Comparisons. All tests treat NULL and any non-NULL string
+// as different. Two NULLs are equal.
+//
+// * {ASSERT|EXPECT}_STREQ(s1, s2): Tests that s1 == s2
+// * {ASSERT|EXPECT}_STRNE(s1, s2): Tests that s1 != s2
+// * {ASSERT|EXPECT}_STRCASEEQ(s1, s2): Tests that s1 == s2, ignoring case
+// * {ASSERT|EXPECT}_STRCASENE(s1, s2): Tests that s1 != s2, ignoring case
+//
+// Macros for comparing floating-point numbers.
+//
+// * {ASSERT|EXPECT}_FLOAT_EQ(expected, actual):
+// Tests that two float values are almost equal.
+// * {ASSERT|EXPECT}_DOUBLE_EQ(expected, actual):
+// Tests that two double values are almost equal.
+// * {ASSERT|EXPECT}_NEAR(v1, v2, abs_error):
+// Tests that v1 and v2 are within the given distance to each other.
+//
+// These predicate format functions work on floating-point values, and
+// can be used in {ASSERT|EXPECT}_PRED_FORMAT2*(), e.g.
+//
+// EXPECT_PRED_FORMAT2(testing::DoubleLE, Foo(), 5.0);
+//
+// Macros that execute statement and check that it doesn't generate new fatal
+// failures in the current thread.
+//
+// * {ASSERT|EXPECT}_NO_FATAL_FAILURE(statement);
+
+// http://stackoverflow.com/a/29155677
+
+namespace testing
+{
+namespace internal
+{
+enum GTestColor
+{
+ COLOR_DEFAULT,
+ COLOR_RED,
+ COLOR_GREEN,
+ COLOR_YELLOW
+};
+
+extern void ColoredPrintf(GTestColor color, const char* fmt, ...);
+
+#define PRINTF(...) \
+ do { testing::internal::ColoredPrintf(testing::internal::COLOR_GREEN,\
+ "[ ] "); \
+ testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); } \
+ while(0)
+
+#define PRINT_TEST_FINISHED \
+{ \
+ const ::testing::TestInfo* const test_info = \
+ ::testing::UnitTest::GetInstance()->current_test_info(); \
+ PRINTF(std::string("Finished test case ").append(test_info->test_case_name()).\
+ append(" of test ").append(test_info->name()).append(".\n").c_str()); \
+}
+
+// C++ stream interface
+class TestCout : public std::stringstream
+{
+public:
+ ~TestCout() override
+ {
+ PRINTF("%s\n", str().c_str());
+ }
+};
+
+/* Usage :
+
+TEST(Test, Foo)
+{
+ // the following works but prints default stream
+ EXPECT_TRUE(false) << "Testing Stream.";
+
+ // or you can play with AINSI color code
+ EXPECT_TRUE(false) << "\033[1;31m" << "Testing Stream.";
+
+ // or use the above defined macros
+
+ PRINTF("Hello world");
+
+ // or
+
+ TEST_COUT << "Hello world";
+}
+
+*/
+#define TEST_COUT testing::internal::TestCout()
+
+} // namespace internal
+
+/* Macros related to testing Eigen classes:
+ */
+#define EXPECT_MATRIX_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+ return (lhs - rhs).isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, C_actual);
+
+#define ASSERT_MATRIX_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+ return (lhs - rhs).isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, C_actual);
+
+#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
+
+#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
+
+#define EXPECT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+ MatrixXd er = lhs - rhs; \
+ er(2) = pi2pi((double)er(2)); \
+ return er.isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, C_actual);
+
+#define ASSERT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+ MatrixXd er = lhs - rhs; \
+ er(2) = pi2pi((double)er(2)); \
+ return er.isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, C_actual);
+
+} // namespace testing
+
+#endif /* GNSSUTILS_UTILS_GTEST_H */
diff --git a/test/gtest_chisquare.cpp b/test/gtest_chisquare.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1c07e12a47b4e9a8f4964f6198a0e7283eef169b
--- /dev/null
+++ b/test/gtest_chisquare.cpp
@@ -0,0 +1,85 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/utils/chisquare_ci.h"
+
+using namespace GnssUtils;
+
+/* chisq_2_CI:
+ * 4 DOF
+ * {1.06362, 0.10}
+ * {1.36648, 0.15},
+ * {1.64878, 0.20},
+ * {1.92256, 0.25},
+ */
+TEST(ChiSquareTest, chisq2ci)
+{
+ // (almost) exact
+ ASSERT_NEAR(chisq2ci(1.36648, 4), 0.15, 1e-12);
+
+ // Before first value
+ ASSERT_NEAR(chisq2ci(0.8, 4), 0.10, 1e-12);
+
+ // Interpolation
+ for (double r = 0; r <= 1; r+=0.1)
+ ASSERT_NEAR(chisq2ci(r*1.36648+(1-r)*1.64878, 4), r*0.15+(1-r)*0.2, 1e-12);
+
+ // negative or zero dof
+ ASSERT_DEATH(chisq2ci(0.4, -2), "");
+ ASSERT_DEATH(chisq2ci(0.4, 0), "");
+
+ // more than 30 dof
+ ASSERT_NEAR(chisq2ci(0.8, 34), chisq2ci(0.8, 30), 1e-12);
+}
+
+/* CI_2_chisq
+ * 4 DOF
+ * {0.10, 1.06362},
+ * {0.15, 1.36648},
+ * {0.20, 1.64878},
+ * {0.25, 1.92256},
+ */
+TEST(ChiSquareTest, ci2chisq)
+{
+ // (almost) exact
+ ASSERT_NEAR(ci2chisq(0.15, 4), 1.36648, 1e-12);
+
+ // Before first value
+ ASSERT_NEAR(ci2chisq(0.05, 4), 1.06362, 1e-12);
+
+ // Interpolation
+ for (double r = 0; r <= 1; r+=0.1)
+ ASSERT_NEAR(ci2chisq(r*0.15+(1-r)*0.2, 4), r*1.36648+(1-r)*1.64878, 1e-12);
+
+ // negative or zero dof
+ ASSERT_DEATH(ci2chisq(0.4, -2), "");
+ ASSERT_DEATH(ci2chisq(0.4, 0), "");
+
+ // more than 30 dof
+ ASSERT_NEAR(ci2chisq(0.8, 34), ci2chisq(0.8, 30), 1e-12);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_example.cpp b/test/gtest_example.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c7059c8f4c9bed9b335a8e18cb88c22241964e98
--- /dev/null
+++ b/test/gtest_example.cpp
@@ -0,0 +1,41 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gtest/utils_gtest.h"
+
+TEST(TestTest, DummyTestExample)
+{
+ EXPECT_FALSE(false);
+
+ ASSERT_TRUE(true);
+
+ int my_int = 5;
+
+ ASSERT_EQ(my_int, 5);
+
+// PRINTF("All good at TestTest::DummyTestExample !\n");
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_navigation.cpp b/test/gtest_navigation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..be3de35ec8cffa98fa3c4d012e1012f0f030d880
--- /dev/null
+++ b/test/gtest_navigation.cpp
@@ -0,0 +1,57 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/utils/utils.h"
+#include "gnss_utils/navigation.h"
+
+using namespace GnssUtils;
+std::string rnx_file = std::string(_GNSS_UTILS_ROOT_DIR) + "/test/data/sample_data.nav";
+const gtime_t t_start{ 0, 0 }; // no limit
+const gtime_t t_end{ 0, 0 }; // no limit
+const double dt = 0.0; // no limit
+const char* opt = ""; // only GPS | GPS+GAL: "-SYS=G,L" | ALL: ""
+
+void loadRinex(nav_t& nav)
+{
+ int stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, NULL, &nav, NULL);
+
+ ASSERT_EQ(stat, 1);
+ uniqnav(&nav);
+}
+
+TEST(NavigationTests, setgetNavigation)
+{
+ nav_t nav;
+ loadRinex(nav);
+
+ Navigation navigation;
+
+ navigation.setNavigation(nav);
+ ASSERT_TRUE(navigation.getNavigation() == nav);
+ Navigation::freeNavArrays(nav);
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_observations.cpp b/test/gtest_observations.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6508d77a7209cf190426a50a16b3c770634e7ca7
--- /dev/null
+++ b/test/gtest_observations.cpp
@@ -0,0 +1,236 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/observations.h"
+
+using namespace GnssUtils;
+std::string rnx_file = std::string(_GNSS_UTILS_ROOT_DIR) + "/test/data/sample_data.obs";
+const gtime_t t_start{ 0, 0 }; // no limit
+const gtime_t t_end{ 0, 0 }; // no limit
+const double dt = 0.0; // no limit
+const char* opt = ""; // only GPS | GPS+GAL: "-SYS=G,L" | ALL: ""
+
+void loadRinex(obs_t& obs)
+{
+ // RTKLIB utilities
+ obs.data = (obsd_t*)malloc(sizeof(obsd_t) * MAXSAT);
+ obs.n = 0;
+ obs.nmax = MAXSAT;
+
+ int stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, &obs, NULL, NULL);
+
+ ASSERT_EQ(stat, 1);
+ sortobs(&obs);
+}
+
+TEST(ObservationsTest, AddClearObservation)
+{
+ obs_t obs;
+ loadRinex(obs);
+
+ Observations observations;
+
+ // testing addition
+ for (int ii = 0; ii < obs.n; ++ii)
+ {
+ observations.addObservation(obs.data[ii]);
+ ASSERT_TRUE(obs.data[ii] == observations.getObservations()[ii]);
+ }
+
+ ASSERT_TRUE(obs.n == observations.getObservations().size());
+ ASSERT_TRUE(obs.n == observations.size());
+
+ // Testing clear
+ observations.clearObservations();
+ ASSERT_TRUE(0 == observations.getObservations().size());
+
+ // Add duplicated observation (has to crash)
+ observations.addObservation(obs.data[0]);
+ ASSERT_DEATH(observations.addObservation(obs.data[0]), "");
+}
+
+TEST(ObservationsTest, LoadFromRinex)
+{
+ // RTKLIB utilities
+ obs_t obs;
+ loadRinex(obs);
+
+ // GnssUtils utilities
+ Observations observations;
+
+ std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+ observations = obs_rinex[0];
+
+ ASSERT_EQ(obs.n, 18);
+
+ // Comparison
+ ASSERT_TRUE(obs.n == observations.getObservations().size());
+ for (int ii = 0; ii < obs.n; ++ii)
+ {
+ ASSERT_TRUE(obs.data[ii] == observations.getObservations()[ii]);
+ }
+
+ free(obs.data);
+}
+
+TEST(ObservationsTest, GetObservationBySat)
+{
+ obs_t obs;
+ loadRinex(obs);
+
+ Observations observations;
+
+ std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+ observations = obs_rinex[0];
+
+ for (int ii = 0; ii < obs.n; ++ii)
+ {
+ ASSERT_TRUE(obs.data[ii] == observations.getObservationBySat(obs.data[ii].sat));
+ }
+ free(obs.data);
+}
+
+TEST(ObservationsTest, GetObservationByIdx)
+{
+ obs_t obs;
+ loadRinex(obs);
+
+ Observations observations;
+
+ std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+ observations = obs_rinex[0];
+
+ for (int ii = 0; ii < obs.n; ++ii)
+ {
+ ASSERT_TRUE(obs.data[ii] == observations.getObservationByIdx(ii));
+ }
+}
+
+TEST(ObservationsTest, data)
+{
+ obs_t obs;
+ loadRinex(obs);
+
+ Observations observations;
+
+ std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+ observations = obs_rinex[0];
+
+ for (int ii = 0; ii < obs.n; ++ii)
+ {
+ ASSERT_TRUE(obs.data[ii] == observations.data()[ii]);
+ }
+}
+
+TEST(ObservationsTest, HasSatellite)
+{
+ obs_t obs;
+ loadRinex(obs);
+
+ Observations observations;
+
+ std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+ observations = obs_rinex[0];
+
+ for (int ii = 0; ii < obs.n; ++ii)
+ {
+ ASSERT_TRUE(observations.hasSatellite(obs.data[ii].sat));
+ }
+}
+
+TEST(ObservationsTest, FindCommonObservations)
+{
+ Observations observations1;
+
+ std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+ observations1 = obs_rinex[0];
+
+ Observations observations2(observations1);
+
+ ASSERT_TRUE(observations1 == observations2);
+
+ std::set<int> common_sats = Observations::findCommonObservations(observations1, observations2);
+
+ ASSERT_TRUE(common_sats.size() == observations1.size());
+ ASSERT_TRUE(common_sats.size() == observations2.size());
+ for (auto sat : common_sats)
+ {
+ ASSERT_TRUE(observations1.hasSatellite(sat));
+ ASSERT_TRUE(observations2.hasSatellite(sat));
+ }
+}
+
+TEST(ObservationsTest, FindCommonObservationsRemoved)
+{
+ Observations observations1;
+
+ std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+ observations1 = obs_rinex[0];
+ Observations observations2(observations1);
+
+ // Remove first observation of observations2
+ int sat_removed = observations2.getObservationByIdx(0).sat;
+ observations2.removeObservationBySat(sat_removed);
+
+ std::set<int> common_sats = Observations::findCommonObservations(observations1, observations2);
+
+ ASSERT_FALSE(common_sats.size() == observations1.size());
+ ASSERT_TRUE(common_sats.size() == observations2.size());
+ for (auto sat : common_sats)
+ {
+ ASSERT_TRUE(observations1.hasSatellite(sat));
+ ASSERT_TRUE(observations2.hasSatellite(sat));
+ }
+}
+
+TEST(ObservationsTest, FindCommonObservationsChangeTime)
+{
+ Observations observations1;
+
+ std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+ observations1 = obs_rinex[0];
+
+ Observations observations2(observations1);
+
+ // Change time
+ for (auto&& obs : observations2.getObservations())
+ {
+ obs.time.sec += 10;
+ }
+
+ std::set<int> common_sats = Observations::findCommonObservations(observations1, observations2);
+
+ ASSERT_TRUE(common_sats.size() == observations1.size());
+ ASSERT_TRUE(common_sats.size() == observations2.size());
+ for (auto sat : common_sats)
+ {
+ ASSERT_TRUE(observations1.hasSatellite(sat));
+ ASSERT_TRUE(observations2.hasSatellite(sat));
+ }
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ // rnx_file = argv[1];
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9db8f2b0fb79c9e127bdaa7ef2ca9dd6894e8732
--- /dev/null
+++ b/test/gtest_tdcp.cpp
@@ -0,0 +1,546 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/tdcp.h"
+#include "gnss_utils/snapshot.h"
+#include "gnss_utils/utils/satellite.h"
+#include "gnss_utils/utils/transformations.h"
+
+using namespace GnssUtils;
+using namespace Eigen;
+
+int N_sats = 20;
+int N_tests = 100;
+double distortion1 = -30;
+double distortion2 = -20;
+
+Vector3d computeRandomReceiverLatLonAlt()
+{
+ Vector3d receiver_LLA = Vector3d::Random(); // in [-1, 1]
+ receiver_LLA(0) *= M_PI / 2; // in [-PI/2, PI/2]
+ receiver_LLA(1) *= M_PI; // in [-PI, PI]
+ receiver_LLA(2) += 1; // ([0, 2])
+ receiver_LLA(2) *= 5e2; // in [0, 1e3]
+
+ return receiver_LLA;
+}
+
+void computeVisibleSatellite(const Vector3d& receiver_latlonalt,
+ const int& n_sat,
+ Vector3d& sat_ENU,
+ Vector3d& sat_ECEF,
+ Vector2d& sat_azel,
+ double& range)
+{
+ Vector3d t_ECEF_ENU, t_ENU_ECEF;
+ Matrix3d R_ENU_ECEF;
+
+ t_ECEF_ENU = latLonAltToEcef(receiver_latlonalt);
+
+ computeEnuEcefFromLatLonAlt(receiver_latlonalt, R_ENU_ECEF, t_ENU_ECEF);
+
+ // equidistributed azimuth at elevation 45º
+ if (n_sat < 6)
+ {
+ sat_azel(0) = -M_PI + n_sat * 2*M_PI/6 + 0.1; // in [-pi, pi]
+ sat_azel(1) = M_PI / 4; // in [0, pi/2]
+ }
+ // random elevation and azimuth
+ else
+ {
+ sat_azel = Vector2d::Random(); // in [-1, 1]
+ sat_azel(0) *= M_PI; // in [-pi, pi]
+ sat_azel(1) = (sat_azel(1)/2 + 0.5) * M_PI / 2; // in [0, pi/2]
+ }
+
+ // random range
+ range = VectorXd::Random(1)(0) * 5e2 + 1e3; // in [500, 1500]
+
+ // ENU
+ sat_ENU << sin(sat_azel(0)) * cos(sat_azel(1)) * range,
+ cos(sat_azel(0)) * cos(sat_azel(1)) * range,
+ sin(sat_azel(1)) * range;
+
+ // ECEF
+ sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * sat_ENU;
+
+ // Range
+ range = (sat_ECEF - t_ECEF_ENU).norm();
+}
+
+TEST(Tdcp, Tdcp)
+{
+ TdcpOptions tdcp_params;
+ tdcp_params.max_iterations = 5;
+ tdcp_params.min_common_sats = 6;
+ tdcp_params.raim_n = 0;
+ tdcp_params.residual_opt = 0;
+ tdcp_params.max_residual_ci = 1e20;
+ tdcp_params.validate_residual = false;
+ tdcp_params.sagnac_correction = false;
+ tdcp_params.relinearize_jacobian = true;
+ tdcp_params.multi_freq = false;
+ tdcp_params.sigma_atm = 1;
+ tdcp_params.sigma_carrier = 1;
+
+ Vector3d sat_ENU, sat_ECEF;
+ Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
+ Matrix3d R_ENU_ECEF;
+ Vector2d azel, azel2;
+ Vector4d d_gt;
+ Matrix4d cov_d;
+ double range, residual_ci;
+
+ // Snapshots
+ auto snapshot_r = std::make_shared<Snapshot>();
+ auto snapshot_k = std::make_shared<Snapshot>();
+
+ // Observations (just needed for the GPST)
+ snapshot_r->setObservations(std::make_shared<Observations>());
+ snapshot_k->setObservations(std::make_shared<Observations>());
+ obsd_t obs_r;
+ obs_r.time.sec = 0;
+ obs_r.time.time = 0;
+ snapshot_r->getObservations()->addObservation(obs_r);
+ obsd_t obs_k;
+ obs_k.time.sec = 0;
+ obs_k.time.time = 1;
+ snapshot_k->getObservations()->addObservation(obs_k);
+
+ // Random receiver position
+ for (auto i = 0; i<N_tests; i++)
+ {
+ // clear satellites and ranges
+ snapshot_r->getSatellites().clear();
+ snapshot_k->getSatellites().clear();
+ snapshot_r->getRanges().clear();
+ snapshot_k->getRanges().clear();
+
+ // random setup
+ x_r_LLA = computeRandomReceiverLatLonAlt();
+ x_r_ECEF = latLonAltToEcef(x_r_LLA);
+ d_ECEF = Vector3d::Random() * 10;
+ x_k_ECEF = x_r_ECEF + d_ECEF;
+ x_k_LLA = ecefToLatLonAlt(x_k_ECEF);
+
+ double clock_r = Vector2d::Random()(0) * 1e-6;
+ double clock_k = Vector2d::Random()(0) * 1e-6;
+
+ // groundtruth
+ d_gt.head<3>() = d_ECEF;
+ d_gt(3) = (clock_k - clock_r) * CLIGHT;
+
+ std::cout << "Test " << i << ":\n";
+ std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n";
+ std::cout << "\tclock_r " << clock_r << ":\n";
+ std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n";
+ std::cout << "\tclock_k " << clock_k << ":\n";
+ std::cout << "\td_gt " << d_gt.transpose() << ":\n";
+
+ std::set<int> common_sats;
+ std::set<int> raim_discarded_sats;
+
+ // random visible satellites
+ for (auto j = 0; j<N_sats; j++)
+ {
+ common_sats.insert(j);
+
+ // Satellite r
+ computeVisibleSatellite(x_r_LLA, j, sat_ENU, sat_ECEF, azel, range);
+ ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3);
+
+ Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
+ snapshot_r->getSatellites().emplace(j, sat_r);
+
+ // Range r
+ Range range_r;
+ range_r.sat = j;
+ range_r.L = range + CLIGHT * clock_r;
+ range_r.L_corrected = range_r.L;
+ range_r.L_var = 1;
+ range_r.L2 = range_r.L;
+ range_r.L2_corrected = range_r.L;
+ range_r.L2_var = range_r.L_var;
+
+ snapshot_r->getRanges().emplace(j, range_r);
+
+ //std::cout << "\tsat: " << j << "\n";
+ //std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
+ //std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
+
+ // Satellite k (random)
+ computeVisibleSatellite(x_k_LLA, j, sat_ENU, sat_ECEF, azel, range);
+ ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3);
+
+ Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
+ snapshot_k->getSatellites().emplace(j, sat_k);
+
+ // Range k
+ Range range_k;
+ range_k.sat = j;
+ range_k.L = range + CLIGHT * clock_k;
+ range_k.L_corrected = range_k.L;
+ range_k.L_var = 1;
+ range_k.L2 = range_k.L;
+ range_k.L2_corrected = range_k.L;
+ range_k.L2_var = range_k.L_var;
+
+ snapshot_k->getRanges().emplace(j, range_k);
+
+ //std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
+ //std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
+ }
+
+ // TDCP
+ auto tdcp_out = Tdcp(snapshot_r,
+ snapshot_k,
+ x_r_ECEF,
+ common_sats,
+ Vector4d::Zero(),
+ tdcp_params);
+
+ // CHECKS
+ std::cout << "CHECKS Test " << i << std::endl;
+ ASSERT_TRUE(tdcp_out.success);
+ ASSERT_LE(tdcp_out.residual_ci, 0.8);
+ ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3);
+ }
+}
+
+TEST(Tdcp, Tdcp_raim_residual_rmse)
+{
+ int N_sats = 20;
+
+ TdcpOptions tdcp_params;
+ tdcp_params.max_iterations = 5;
+ tdcp_params.min_common_sats = 6;
+ tdcp_params.raim_n = 2;
+ tdcp_params.residual_opt = 0; // Normalized RMSE
+ tdcp_params.max_residual_ci = 0.1; // low threshold to detect outliers...
+ tdcp_params.validate_residual = false;
+ tdcp_params.sagnac_correction = false;
+ tdcp_params.relinearize_jacobian = true;
+ tdcp_params.multi_freq = false;
+ tdcp_params.sigma_atm = 1;
+ tdcp_params.sigma_carrier = 1;
+
+
+ Vector3d sat_ENU, sat_ECEF;
+ Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
+ Matrix3d R_ENU_ECEF;
+ Vector2d azel, azel2;
+ Vector4d d, d_gt;
+ Matrix4d cov_d;
+ double range, residual_ci;
+
+ // Snapshots
+ auto snapshot_r = std::make_shared<Snapshot>();
+ auto snapshot_k = std::make_shared<Snapshot>();
+
+ // Observations (just needed for the GPST)
+ snapshot_r->setObservations(std::make_shared<Observations>());
+ snapshot_k->setObservations(std::make_shared<Observations>());
+ obsd_t obs_r;
+ obs_r.time.sec = 0;
+ obs_r.time.time = 0;
+ snapshot_r->getObservations()->addObservation(obs_r);
+ obsd_t obs_k;
+ obs_k.time.sec = 0;
+ obs_k.time.time = 1;
+ snapshot_k->getObservations()->addObservation(obs_k);
+
+ // Random receiver position
+ for (auto i = 0; i<N_tests; i++)
+ {
+ // clear satellites and ranges
+ snapshot_r->getSatellites().clear();
+ snapshot_k->getSatellites().clear();
+ snapshot_r->getRanges().clear();
+ snapshot_k->getRanges().clear();
+
+ // random setup
+ x_r_LLA = computeRandomReceiverLatLonAlt();
+ x_r_ECEF = latLonAltToEcef(x_r_LLA);
+ d_ECEF = Vector3d::Random() * 10;
+ x_k_ECEF = x_r_ECEF + d_ECEF;
+ x_k_LLA = ecefToLatLonAlt(x_k_ECEF);
+
+ double clock_r = Vector2d::Random()(0) * 1e-6;
+ double clock_k = Vector2d::Random()(0) * 1e-6;
+
+ // groundtruth
+ d_gt.head<3>() = d_ECEF;
+ d_gt(3) = (clock_k - clock_r) * CLIGHT;
+
+ // distorted sats
+ int wrong_sat1 = i % N_sats;
+ int wrong_sat2 = (i + N_sats / 2) % N_sats;
+
+ std::cout << "Test " << i << ":\n";
+ std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n";
+ std::cout << "\tclock_r " << clock_r << ":\n";
+ std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n";
+ std::cout << "\tclock_k " << clock_k << ":\n";
+ std::cout << "\td_gt " << d_gt.transpose() << ":\n";
+ std::cout << "\tdistorted sats: " << wrong_sat1 << ", " << wrong_sat2 << std::endl;
+
+ std::set<int> common_sats;
+ std::set<int> raim_discarded_sats;
+
+ // random visible satellites
+ for (auto j = 0; j<N_sats; j++)
+ {
+ common_sats.insert(j);
+
+ // Satellite r (random)
+ computeVisibleSatellite(x_r_LLA, j, sat_ENU, sat_ECEF, azel, range);
+ ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3);
+
+ Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
+ snapshot_r->getSatellites().emplace(j, sat_r);
+
+ // Range r
+ Range range_r;
+ range_r.sat = j;
+ range_r.L = range + CLIGHT * clock_r;
+ range_r.L_corrected = range_r.L;
+ range_r.L_var = 1;
+ range_r.L2 = range_r.L;
+ range_r.L2_corrected = range_r.L;
+ range_r.L2_var = range_r.L_var;
+
+ snapshot_r->getRanges().emplace(j, range_r);
+
+ //std::cout << "\tsat: " << j << "\n";
+ //std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
+ //std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
+
+ // Satellite k (random)
+ computeVisibleSatellite(x_k_LLA, j, sat_ENU, sat_ECEF, azel, range);
+ ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3);
+
+ Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
+ snapshot_k->getSatellites().emplace(j, sat_k);
+
+ // Range k
+ Range range_k;
+ range_k.sat = j;
+ range_k.L = range + CLIGHT * clock_k;
+ range_k.L_corrected = range_k.L;
+ range_k.L_var = 1;
+ range_k.L2 = range_k.L;
+ range_k.L2_corrected = range_k.L;
+ range_k.L2_var = range_k.L_var;
+
+ snapshot_k->getRanges().emplace(j, range_k);
+
+ //std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
+ //std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
+ }
+
+ // Distort 2 ranges -> to be detected by RAIM
+ snapshot_k->getRanges().at(wrong_sat1).L_corrected += distortion1;
+ snapshot_k->getRanges().at(wrong_sat1).L += distortion1;
+ snapshot_r->getRanges().at(wrong_sat2).L_corrected += distortion2;
+ snapshot_r->getRanges().at(wrong_sat2).L += distortion2;
+
+
+ // TDCP
+ auto tdcp_out = Tdcp(snapshot_r,
+ snapshot_k,
+ x_r_ECEF,
+ common_sats,
+ Vector4d::Zero(),
+ tdcp_params);
+
+ // CHECKS
+ std::cout << "CHECKS iteration " << i << std::endl;
+ ASSERT_EQ(tdcp_out.raim_discarded_sats.size(), 2);
+ ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat1));
+ ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat2));
+ ASSERT_TRUE(tdcp_out.success);
+ ASSERT_LE(tdcp_out.residual_ci, 0.8);
+ ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3);
+ }
+}
+
+TEST(Tdcp, Tdcp_raim_residual_max_mah)
+{
+ int N_sats = 20;
+
+ TdcpOptions tdcp_params;
+ tdcp_params.max_iterations = 5;
+ tdcp_params.min_common_sats = 6;
+ tdcp_params.raim_n = 2;
+ tdcp_params.residual_opt = 1; // Max residual_ci in Mahalanobis distance
+ tdcp_params.max_residual_ci = 0.95; // 95% of confidence
+ tdcp_params.validate_residual = false;
+ tdcp_params.sagnac_correction = false;
+ tdcp_params.relinearize_jacobian = true;
+ tdcp_params.multi_freq = false;
+ tdcp_params.sigma_atm = 1;
+ tdcp_params.sigma_carrier = 1;
+
+
+ Vector3d sat_ENU, sat_ECEF;
+ Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
+ Matrix3d R_ENU_ECEF;
+ Vector2d azel, azel2;
+ Vector4d d, d_gt;
+ Matrix4d cov_d;
+ double range, residual_ci;
+
+ // Snapshots
+ auto snapshot_r = std::make_shared<Snapshot>();
+ auto snapshot_k = std::make_shared<Snapshot>();
+
+ // Observations (just needed for the GPST)
+ snapshot_r->setObservations(std::make_shared<Observations>());
+ snapshot_k->setObservations(std::make_shared<Observations>());
+ obsd_t obs_r;
+ obs_r.time.sec = 0;
+ obs_r.time.time = 0;
+ snapshot_r->getObservations()->addObservation(obs_r);
+ obsd_t obs_k;
+ obs_k.time.sec = 0;
+ obs_k.time.time = 1;
+ snapshot_k->getObservations()->addObservation(obs_k);
+
+ // Random receiver position
+ for (auto i = 0; i<N_tests; i++)
+ {
+ // clear satellites and ranges
+ snapshot_r->getSatellites().clear();
+ snapshot_k->getSatellites().clear();
+ snapshot_r->getRanges().clear();
+ snapshot_k->getRanges().clear();
+
+ // random setup
+ x_r_LLA = computeRandomReceiverLatLonAlt();
+ x_r_ECEF = latLonAltToEcef(x_r_LLA);
+ d_ECEF = Vector3d::Random() * 10;
+ x_k_ECEF = x_r_ECEF + d_ECEF;
+ x_k_LLA = ecefToLatLonAlt(x_k_ECEF);
+
+ double clock_r = Vector2d::Random()(0) * 1e-6;
+ double clock_k = Vector2d::Random()(0) * 1e-6;
+
+ // groundtruth
+ d_gt.head<3>() = d_ECEF;
+ d_gt(3) = (clock_k - clock_r) * CLIGHT;
+
+ // distorted sats
+ int wrong_sat1 = i % N_sats;
+ int wrong_sat2 = (i + N_sats / 2) % N_sats;
+
+ std::cout << "\nTest " << i << ":\n";
+ std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n";
+ std::cout << "\tclock_r " << clock_r << ":\n";
+ std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n";
+ std::cout << "\tclock_k " << clock_k << ":\n";
+ std::cout << "\td_gt " << d_gt.transpose() << ":\n";
+ std::cout << "\tdistorted sats: " << wrong_sat1 << ", " << wrong_sat2 << std::endl;
+
+ std::set<int> common_sats;
+ std::set<int> raim_discarded_sats;
+
+ // random visible satellites
+ for (auto j = 0; j<N_sats; j++)
+ {
+ common_sats.insert(j);
+
+ // Satellite r (random)
+ computeVisibleSatellite(x_r_LLA, j, sat_ENU, sat_ECEF, azel, range);
+ ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3);
+
+ Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
+ snapshot_r->getSatellites().emplace(j, sat_r);
+
+ // Range r
+ Range range_r;
+ range_r.sat = j;
+ range_r.L = range + CLIGHT * clock_r;
+ range_r.L_corrected = range_r.L;
+ range_r.L_var = 1;
+ range_r.L2 = range_r.L;
+ range_r.L2_corrected = range_r.L;
+ range_r.L2_var = range_r.L_var;
+
+ snapshot_r->getRanges().emplace(j, range_r);
+
+ //std::cout << "\tsat: " << j << "\n";
+ //std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
+ //std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
+
+ // Satellite k (random)
+ computeVisibleSatellite(x_k_LLA, j, sat_ENU, sat_ECEF, azel, range);
+ ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3);
+
+ Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
+ snapshot_k->getSatellites().emplace(j, sat_k);
+
+ // Range k
+ Range range_k;
+ range_k.sat = j;
+ range_k.L = range + CLIGHT * clock_k;
+ range_k.L_corrected = range_k.L;
+ range_k.L_var = 1;
+ range_k.L2 = range_k.L;
+ range_k.L2_corrected = range_k.L;
+ range_k.L2_var = range_k.L_var;
+
+ snapshot_k->getRanges().emplace(j, range_k);
+
+ //std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
+ //std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
+ }
+
+ // Distort 2 ranges -> to be detected by RAIM
+ snapshot_k->getRanges().at(wrong_sat1).L_corrected += distortion1;
+ snapshot_k->getRanges().at(wrong_sat1).L += distortion1;
+ snapshot_r->getRanges().at(wrong_sat2).L_corrected += distortion2;
+ snapshot_r->getRanges().at(wrong_sat2).L += distortion2;
+
+ // TDCP
+ auto tdcp_out = Tdcp(snapshot_r,
+ snapshot_k,
+ x_r_ECEF,
+ common_sats,
+ Vector4d::Zero(),
+ tdcp_params);
+
+ // CHECKS
+ std::cout << "Checks iteration " << i << std::endl;
+ ASSERT_EQ(tdcp_out.raim_discarded_sats.size(), 2);
+ ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat1));
+ ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat2));
+ ASSERT_TRUE(tdcp_out.success);
+ ASSERT_LE(tdcp_out.residual_ci, 0.8);
+ ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_transformations.cpp b/test/gtest_transformations.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..76162d6c39f1295ce5cbec34a3fa919d50c88389
--- /dev/null
+++ b/test/gtest_transformations.cpp
@@ -0,0 +1,246 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of gnss_utils
+// gnss_utils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/utils/satellite.h"
+#include "gnss_utils/utils/transformations.h"
+
+// Geodetic system parameters
+static double kSemimajorAxis = 6378137;
+static double kSemiminorAxis = 6356752.3142;
+static double kFirstEccentricitySquared = 6.69437999014 * 0.001;
+static double kSecondEccentricitySquared = 6.73949674228 * 0.001;
+
+using namespace GnssUtils;
+
+Eigen::Vector3d computeRandomReceiverLatLonAlt()
+{
+ Eigen::Vector3d receiver_LLA = Eigen::Vector3d::Random(); // in [-1, 1]
+ receiver_LLA(0) *= M_PI / 2; // in [-PI/2, PI/2]
+ receiver_LLA(1) *= M_PI; // in [-PI, PI]
+ receiver_LLA(2) += 1; // ([0, 2])
+ receiver_LLA(2) *= 5e2; // in [0, 1e3]
+
+ return receiver_LLA;
+}
+
+void computeRandomVisibleSatellite(const Eigen::Vector3d& receiver_latlonalt,
+ Eigen::Vector3d& sat_ENU,
+ Eigen::Vector3d& sat_ECEF,
+ Eigen::Vector2d& sat_azel,
+ double range)
+{
+ Eigen::Vector3d t_ECEF_ENU, t_ENU_ECEF;
+ Eigen::Matrix3d R_ENU_ECEF;
+
+ t_ECEF_ENU = latLonAltToEcef(receiver_latlonalt);
+
+ computeEnuEcefFromLatLonAlt(receiver_latlonalt, R_ENU_ECEF, t_ENU_ECEF);
+
+ // random elevation and azimuth
+ sat_azel = Eigen::Vector2d::Random(); // in [-1, 1]
+ sat_azel(0) *= M_PI; // in [-pi, pi]
+ sat_azel(1) = (sat_azel(1)/2 + 0.5) * M_PI / 2; // in [0, pi/2]
+ range = Eigen::VectorXd::Random(1)(0) * 5e2 + 1e3; // in [500, 1500]
+
+ // ENU
+ sat_ENU << sin(sat_azel(0)) * cos(sat_azel(1)) * range,
+ cos(sat_azel(0)) * cos(sat_azel(1)) * range,
+ sin(sat_azel(1)) * range;
+
+ // ECEF
+ sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * sat_ENU;
+}
+
+
+TEST(TransformationsTest, ecefToLatLonAlt)
+{
+ Eigen::Vector3d ecef, latlonalt;
+
+ // aligned with ECEF X axis
+ ecef << 1e3, 0, 0;
+ latlonalt = ecefToLatLonAlt(ecef);
+
+ ASSERT_NEAR(latlonalt(0),0.0,1e-6);
+ ASSERT_NEAR(latlonalt(1),0.0,1e-6);
+
+ // aligned with ECEF Y axis
+ ecef << 0, 1e3, 0;
+ latlonalt = ecefToLatLonAlt(ecef);
+
+ ASSERT_NEAR(latlonalt(0),0.0,1e-6);
+ ASSERT_NEAR(latlonalt(1),M_PI / 2,1e-6);
+
+ // aligned with ECEF Z axis
+ ecef << 0, 0, 1e3;
+ latlonalt = ecefToLatLonAlt(ecef);
+
+ ASSERT_NEAR(latlonalt(0),M_PI / 2,1e-6);
+ ASSERT_NEAR(latlonalt(1),0.0,1e-6);
+}
+
+TEST(TransformationsTest, latLonAltToEcef)
+{
+ Eigen::Vector3d ecef, latlonalt;
+
+ // aligned with ECEF X axis
+ latlonalt << 0, 0, 0;
+ ecef = latLonAltToEcef(latlonalt);
+
+ ASSERT_NEAR(ecef(1),0.0,1e-6);
+ ASSERT_NEAR(ecef(2),0.0,1e-6);
+
+ // aligned with ECEF Y axis
+ latlonalt << 0, M_PI / 2, 0;
+ ecef = latLonAltToEcef(latlonalt);
+
+ ASSERT_NEAR(ecef(0),0.0,1e-6);
+ ASSERT_NEAR(ecef(2),0.0,1e-6);
+
+ // aligned with ECEF Z axis
+ latlonalt << M_PI / 2, 0, 0;
+ ecef = latLonAltToEcef(latlonalt);
+
+ ASSERT_NEAR(ecef(0),0.0,1e-6);
+ ASSERT_NEAR(ecef(1),0.0,1e-6);
+}
+
+TEST(TransformationsTest, latLonAltToEcef_ecefToLatLonAlt)
+{
+ Eigen::Vector3d ecef0, ecef1, latlonalt0, latlonalt1;
+
+ // ecef -> latlon -> ecef
+ for (auto i = 0; i<100; i++)
+ {
+ ecef0 = 1e3 * Eigen::Vector3d::Random();
+
+ latlonalt0 = ecefToLatLonAlt(ecef0);
+ ecef1 = latLonAltToEcef(latlonalt0);
+ ASSERT_MATRIX_APPROX(ecef0,ecef1,1e-6);
+ }
+ // latlon -> ecef -> latlon
+ for (auto i = 0; i<100; i++)
+ {
+ latlonalt0 = computeRandomReceiverLatLonAlt();
+
+ ecef0 = latLonAltToEcef(latlonalt0);
+ latlonalt1 = ecefToLatLonAlt(ecef0);
+ ASSERT_MATRIX_APPROX(latlonalt0,latlonalt1,1e-6);
+ }
+}
+
+TEST(TransformationsTest, computeEnuEcef)
+{
+ Eigen::Vector3d t_ECEF_ENU, t_ENU_latlonalt, latlonalt1;
+ Eigen::Vector3d t_ENU_ECEF1,t_ENU_ECEF2,t_ENU_ECEF3;
+ Eigen::Matrix3d R_ENU_ECEF1,R_ENU_ECEF2,R_ENU_ECEF3;
+
+ // random
+ for (auto i = 0; i<100; i++)
+ {
+ t_ENU_latlonalt = computeRandomReceiverLatLonAlt();
+
+ t_ECEF_ENU = latLonAltToEcef(t_ENU_latlonalt);
+
+ // 1 from ECEF
+ computeEnuEcefFromEcef(t_ECEF_ENU, R_ENU_ECEF1, t_ENU_ECEF1);
+ // 2 from latlonalt
+ computeEnuEcefFromLatLonAlt(t_ENU_latlonalt, R_ENU_ECEF2, t_ENU_ECEF2);
+ // 3 Hardcoded solution
+ /* Convert ECEF coordinates to geodetic coordinates.
+ * J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
+ * to geodetic coordinates," IEEE Transactions on Aerospace and
+ * Electronic Systems, vol. 30, pp. 957-961, 1994.
+ */
+ double r = std::sqrt(t_ECEF_ENU(0) * t_ECEF_ENU(0) + t_ECEF_ENU(1) * t_ECEF_ENU(1));
+ double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
+ double F = 54 * kSemiminorAxis * kSemiminorAxis * t_ECEF_ENU(2) * t_ECEF_ENU(2);
+ double G = r * r + (1 - kFirstEccentricitySquared) * t_ECEF_ENU(2) * t_ECEF_ENU(2) - kFirstEccentricitySquared * Esq;
+ double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
+ double S = cbrt(1 + C + sqrt(C * C + 2 * C));
+ double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
+ double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
+ double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q)
+ + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
+ - P * (1 - kFirstEccentricitySquared) * t_ECEF_ENU(2) * t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P * r * r);
+ double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * t_ECEF_ENU(2) * t_ECEF_ENU(2));
+ double Z_0 = kSemiminorAxis * kSemiminorAxis * t_ECEF_ENU(2) / (kSemimajorAxis * V);
+
+ double latitude = atan((t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r);
+ double longitude = atan2(t_ECEF_ENU(1), t_ECEF_ENU(0));
+
+ double sLat = sin(latitude);
+ double cLat = cos(latitude);
+ double sLon = sin(longitude);
+ double cLon = cos(longitude);
+
+ R_ENU_ECEF3(0,0) = -sLon;
+ R_ENU_ECEF3(0,1) = cLon;
+ R_ENU_ECEF3(0,2) = 0.0;
+
+ R_ENU_ECEF3(1,0) = -sLat*cLon;
+ R_ENU_ECEF3(1,1) = -sLat * sLon;
+ R_ENU_ECEF3(1,2) = cLat;
+
+ R_ENU_ECEF3(2,0) = cLat * cLon;
+ R_ENU_ECEF3(2,1) = cLat * sLon;
+ R_ENU_ECEF3(2,2) = sLat;
+
+ t_ENU_ECEF3 = -R_ENU_ECEF3*t_ECEF_ENU;
+
+ // CHECK
+ ASSERT_MATRIX_APPROX(t_ENU_ECEF1,t_ENU_ECEF2,1e-6);
+ ASSERT_MATRIX_APPROX(R_ENU_ECEF1,R_ENU_ECEF2,1e-6);
+ ASSERT_MATRIX_APPROX(t_ENU_ECEF1,t_ENU_ECEF3,1e-6);
+ ASSERT_MATRIX_APPROX(R_ENU_ECEF1,R_ENU_ECEF3,1e-6);
+ }
+}
+
+TEST(TransformationsTest, computeAzel)
+{
+ Eigen::Vector3d t_ENU_ECEF,t_ECEF_ENU,t_ENU_latlonalt, sat_ENU, sat_ECEF;
+ Eigen::Matrix3d R_ENU_ECEF;
+ Eigen::Vector2d azel, azel2;
+ double range;
+
+ // random receiver position
+ for (auto i = 0; i<100; i++)
+ {
+ t_ENU_latlonalt = computeRandomReceiverLatLonAlt();
+ t_ECEF_ENU = latLonAltToEcef(t_ENU_latlonalt);
+
+ // random elevation and heading
+ for (auto j = 0; j<100; j++)
+ {
+ computeRandomVisibleSatellite(t_ENU_latlonalt, sat_ENU, sat_ECEF, azel, range);
+
+ azel2 = computeAzel(sat_ECEF, t_ECEF_ENU);
+
+ ASSERT_MATRIX_APPROX(azel,azel2,1e-6);
+ }
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}