├── .gitignore ├── 3rdparty ├── cxxopts.hpp ├── easylogging++.cpp ├── easylogging++.h ├── enum.h ├── ghc │ ├── filesystem.hpp │ ├── fs_fwd.hpp │ ├── fs_impl.hpp │ ├── fs_std.hpp │ ├── fs_std_fwd.hpp │ └── fs_std_impl.hpp └── jprogress_bar.hpp ├── CMakeLists.txt ├── README.md ├── app └── cui │ ├── CMakeLists.txt │ ├── TGINS.cc │ └── compareSol.cc ├── config ├── 100C3640.yaml ├── 100C3640_PPP.yaml └── 64650140.yaml ├── doc └── source │ ├── 6465 │ ├── all.mp4 │ ├── att_err.png │ ├── ba.png │ ├── bg.png │ ├── pos_err.png │ ├── stat_err.png │ └── vel_err.png │ ├── 100C │ ├── lc-obs │ │ ├── all.mp4 │ │ ├── att_err.png │ │ ├── ba.png │ │ ├── bg.png │ │ ├── pos_err.png │ │ ├── stat_err.png │ │ └── vel_err.png │ ├── stc-ppk │ │ ├── all.mp4 │ │ ├── att_err.png │ │ ├── pos_err.png │ │ ├── stat_err.png │ │ └── vel_err.png │ ├── tc-ppk │ │ ├── all.mp4 │ │ ├── att_err.png │ │ ├── pos_err.png │ │ ├── stat_err.png │ │ └── vel_err.png │ └── tc-ppp │ │ ├── all.mp4 │ │ ├── att_err.png │ │ ├── ba.png │ │ ├── bg.png │ │ ├── pos_err.png │ │ ├── stat_err.png │ │ └── vel_err.png │ └── dir │ ├── doy_dir.png │ ├── main_dir.png │ ├── obs_dir.png │ ├── products_dir.png │ └── year_dir.png ├── example └── data.txt ├── plot ├── FUSING-PLOT └── rtkplot_qt.ini └── src ├── common ├── CMakeLists.txt ├── Config.cc ├── Config.h ├── Constants.h ├── Coordinate.cc ├── Coordinate.h ├── EigenInc.h ├── Enums.h ├── GTime.cc ├── GTime.h ├── Logger.cc ├── Logger.h ├── ProcessBar.cc ├── ProcessBar.h ├── Rotation.cc ├── Rotation.h ├── Statistics.cc └── Statistics.h ├── fusing ├── CMakeLists.txt ├── Fbs.cc ├── Fbs.h ├── Fusing.cc ├── Fusing.h ├── LooseCouple.cc ├── LooseCouple.h ├── Rts.cc ├── Rts.h ├── SyncSensors.cc ├── SyncSensors.h ├── TightCouple.cc ├── TightCouple.h ├── TimeUpdate.cc └── TimeUpdate.h ├── misc ├── CMakeLists.txt ├── MatchFiles.cc ├── MatchFiles.h ├── OutSol.cc ├── OutSol.h ├── RefSol.cc └── RefSol.h └── sensors ├── gnss ├── CMakeLists.txt ├── Gnss.cc ├── Gnss.h └── rtklib │ ├── convgpx.cc │ ├── convkml.cc │ ├── convrnx.cc │ ├── datum.cc │ ├── download.cc │ ├── ephemeris.cc │ ├── geoid.cc │ ├── gis.cc │ ├── ionex.cc │ ├── lambda.cc │ ├── options.cc │ ├── pntpos.cc │ ├── postpos.cc │ ├── ppp.cc │ ├── ppp_ar.cc │ ├── preceph.cc │ ├── rcv │ ├── binex.cc │ ├── javad.cc │ ├── novatel.cc │ ├── nvs.cc │ ├── rt17.cc │ ├── septentrio.cc │ ├── skytraq.cc │ ├── ss2.cpp │ └── ublox.cc │ ├── rcvraw.cc │ ├── rinex.cc │ ├── rtcm.cc │ ├── rtcm2.cc │ ├── rtcm3.cc │ ├── rtcm3e.cc │ ├── rtkcmn.cc │ ├── rtklib.h │ ├── rtkpos.cc │ ├── rtksvr.cc │ ├── sbas.cc │ ├── solution.cc │ ├── stream.cc │ ├── streamsvr.cc │ ├── tdcp.cc │ ├── tdcp1.cc │ ├── tides.cc │ └── tle.cc └── imu ├── CMakeLists.txt ├── Imu.cc ├── Imu.h ├── Ins.cc └── Ins.h /.gitignore: -------------------------------------------------------------------------------- 1 | # Project exclude paths 2 | /cmake-build-debug/ 3 | /build/ 4 | /.idea 5 | -------------------------------------------------------------------------------- /3rdparty/ghc/fs_fwd.hpp: -------------------------------------------------------------------------------- 1 | //--------------------------------------------------------------------------------------- 2 | // 3 | // ghc::filesystem - A C++17-like filesystem implementation for C++11/C++14 4 | // 5 | //--------------------------------------------------------------------------------------- 6 | // 7 | // Copyright (c) 2018, Steffen Schümann 8 | // 9 | // Permission is hereby granted, free of charge, to any person obtaining a copy 10 | // of this software and associated documentation files (the "Software"), to deal 11 | // in the Software without restriction, including without limitation the rights 12 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | // copies of the Software, and to permit persons to whom the Software is 14 | // furnished to do so, subject to the following conditions: 15 | // 16 | // The above copyright notice and this permission notice shall be included in all 17 | // copies or substantial portions of the Software. 18 | // 19 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | // SOFTWARE. 26 | // 27 | //--------------------------------------------------------------------------------------- 28 | // fs_fwd.hpp - The forwarding header for the header/implementation seperated usage of 29 | // ghc::filesystem. 30 | // This file can be include at any place, where ghc::filesystem api is needed while 31 | // not bleeding implementation details (e.g. system includes) into the global namespace, 32 | // as long as one cpp includes fs_impl.hpp to deliver the matching implementations. 33 | //--------------------------------------------------------------------------------------- 34 | #ifndef GHC_FILESYSTEM_FWD_H 35 | #define GHC_FILESYSTEM_FWD_H 36 | #define GHC_FILESYSTEM_FWD 37 | #include 38 | #endif // GHC_FILESYSTEM_FWD_H 39 | -------------------------------------------------------------------------------- /3rdparty/ghc/fs_impl.hpp: -------------------------------------------------------------------------------- 1 | //--------------------------------------------------------------------------------------- 2 | // 3 | // ghc::filesystem - A C++17-like filesystem implementation for C++11/C++14 4 | // 5 | //--------------------------------------------------------------------------------------- 6 | // 7 | // Copyright (c) 2018, Steffen Schümann 8 | // 9 | // Permission is hereby granted, free of charge, to any person obtaining a copy 10 | // of this software and associated documentation files (the "Software"), to deal 11 | // in the Software without restriction, including without limitation the rights 12 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | // copies of the Software, and to permit persons to whom the Software is 14 | // furnished to do so, subject to the following conditions: 15 | // 16 | // The above copyright notice and this permission notice shall be included in all 17 | // copies or substantial portions of the Software. 18 | // 19 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | // SOFTWARE. 26 | // 27 | //--------------------------------------------------------------------------------------- 28 | // fs_impl.hpp - The implementation header for the header/implementation seperated usage of 29 | // ghc::filesystem. 30 | // This file can be used to hide the implementation of ghc::filesystem into a single cpp. 31 | // The cpp has to include this before including fs_fwd.hpp directly or via a different 32 | // header to work. 33 | //--------------------------------------------------------------------------------------- 34 | #define GHC_FILESYSTEM_IMPLEMENTATION 35 | #include 36 | -------------------------------------------------------------------------------- /3rdparty/ghc/fs_std.hpp: -------------------------------------------------------------------------------- 1 | //--------------------------------------------------------------------------------------- 2 | // 3 | // ghc::filesystem - A C++17-like filesystem implementation for C++11/C++14 4 | // 5 | //--------------------------------------------------------------------------------------- 6 | // 7 | // Copyright (c) 2018, Steffen Schümann 8 | // 9 | // Permission is hereby granted, free of charge, to any person obtaining a copy 10 | // of this software and associated documentation files (the "Software"), to deal 11 | // in the Software without restriction, including without limitation the rights 12 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | // copies of the Software, and to permit persons to whom the Software is 14 | // furnished to do so, subject to the following conditions: 15 | // 16 | // The above copyright notice and this permission notice shall be included in all 17 | // copies or substantial portions of the Software. 18 | // 19 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | // SOFTWARE. 26 | // 27 | //--------------------------------------------------------------------------------------- 28 | // fs_std.hpp - The dynamic switching header that includes std::filesystem if detected 29 | // or ghc::filesystem if not, and makes the resulting API available in the 30 | // namespace fs. 31 | //--------------------------------------------------------------------------------------- 32 | #ifndef GHC_FILESYSTEM_STD_H 33 | #define GHC_FILESYSTEM_STD_H 34 | #if defined(__APPLE__) 35 | #include 36 | #endif 37 | #if ((defined(_MSVC_LANG) && _MSVC_LANG >= 201703L) || (defined(__cplusplus) && __cplusplus >= 201703L)) && defined(__has_include) 38 | #if __has_include() && (!defined(__MAC_OS_X_VERSION_MIN_REQUIRED) || __MAC_OS_X_VERSION_MIN_REQUIRED >= 101500) 39 | #define GHC_USE_STD_FS 40 | #include 41 | namespace fs { 42 | using namespace std::filesystem; 43 | using ifstream = std::ifstream; 44 | using ofstream = std::ofstream; 45 | using fstream = std::fstream; 46 | } 47 | #endif 48 | #endif 49 | #ifndef GHC_USE_STD_FS 50 | //#define GHC_WIN_DISABLE_WSTRING_STORAGE_TYPE 51 | #include 52 | namespace fs { 53 | using namespace ghc::filesystem; 54 | using ifstream = ghc::filesystem::ifstream; 55 | using ofstream = ghc::filesystem::ofstream; 56 | using fstream = ghc::filesystem::fstream; 57 | } 58 | #endif 59 | #endif // GHC_FILESYSTEM_STD_H 60 | 61 | -------------------------------------------------------------------------------- /3rdparty/ghc/fs_std_fwd.hpp: -------------------------------------------------------------------------------- 1 | //--------------------------------------------------------------------------------------- 2 | // 3 | // ghc::filesystem - A C++17-like filesystem implementation for C++11/C++14 4 | // 5 | //--------------------------------------------------------------------------------------- 6 | // 7 | // Copyright (c) 2018, Steffen Schümann 8 | // 9 | // Permission is hereby granted, free of charge, to any person obtaining a copy 10 | // of this software and associated documentation files (the "Software"), to deal 11 | // in the Software without restriction, including without limitation the rights 12 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | // copies of the Software, and to permit persons to whom the Software is 14 | // furnished to do so, subject to the following conditions: 15 | // 16 | // The above copyright notice and this permission notice shall be included in all 17 | // copies or substantial portions of the Software. 18 | // 19 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | // SOFTWARE. 26 | // 27 | //--------------------------------------------------------------------------------------- 28 | // fs_std_fwd.hpp - The forwarding header for the header/implementation seperated usage of 29 | // ghc::filesystem that uses std::filesystem if it detects it. 30 | // This file can be include at any place, where fs::filesystem api is needed while 31 | // not bleeding implementation details (e.g. system includes) into the global namespace, 32 | // as long as one cpp includes fs_std_impl.hpp to deliver the matching implementations. 33 | //--------------------------------------------------------------------------------------- 34 | #ifndef GHC_FILESYSTEM_STD_FWD_H 35 | #define GHC_FILESYSTEM_STD_FWD_H 36 | #if defined(__APPLE__) 37 | #include 38 | #endif 39 | #if ((defined(_MSVC_LANG) && _MSVC_LANG >= 201703L) || (defined(__cplusplus) && __cplusplus >= 201703L)) && defined(__has_include) 40 | #if __has_include() && (!defined(__MAC_OS_X_VERSION_MIN_REQUIRED) || __MAC_OS_X_VERSION_MIN_REQUIRED >= 101500) 41 | #define GHC_USE_STD_FS 42 | #include 43 | namespace fs { 44 | using namespace std::filesystem; 45 | using ifstream = std::ifstream; 46 | using ofstream = std::ofstream; 47 | using fstream = std::fstream; 48 | } 49 | #endif 50 | #endif 51 | #ifndef GHC_USE_STD_FS 52 | //#define GHC_WIN_DISABLE_WSTRING_STORAGE_TYPE 53 | #define GHC_FILESYSTEM_FWD 54 | #include 55 | namespace fs { 56 | using namespace ghc::filesystem; 57 | using ifstream = ghc::filesystem::ifstream; 58 | using ofstream = ghc::filesystem::ofstream; 59 | using fstream = ghc::filesystem::fstream; 60 | } 61 | #endif 62 | #endif // GHC_FILESYSTEM_STD_FWD_H 63 | 64 | -------------------------------------------------------------------------------- /3rdparty/ghc/fs_std_impl.hpp: -------------------------------------------------------------------------------- 1 | //--------------------------------------------------------------------------------------- 2 | // 3 | // ghc::filesystem - A C++17-like filesystem implementation for C++11/C++14 4 | // 5 | //--------------------------------------------------------------------------------------- 6 | // 7 | // Copyright (c) 2018, Steffen Schümann 8 | // 9 | // Permission is hereby granted, free of charge, to any person obtaining a copy 10 | // of this software and associated documentation files (the "Software"), to deal 11 | // in the Software without restriction, including without limitation the rights 12 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | // copies of the Software, and to permit persons to whom the Software is 14 | // furnished to do so, subject to the following conditions: 15 | // 16 | // The above copyright notice and this permission notice shall be included in all 17 | // copies or substantial portions of the Software. 18 | // 19 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | // SOFTWARE. 26 | // 27 | //--------------------------------------------------------------------------------------- 28 | // fs_std_impl.hpp - The implementation header for the header/implementation seperated usage of 29 | // ghc::filesystem that does nothing if std::filesystem is detected. 30 | // This file can be used to hide the implementation of ghc::filesystem into a single cpp. 31 | // The cpp has to include this before including fs_std_fwd.hpp directly or via a different 32 | // header to work. 33 | //--------------------------------------------------------------------------------------- 34 | #if defined(__APPLE__) 35 | #include 36 | #endif 37 | #if ((defined(_MSVC_LANG) && _MSVC_LANG >= 201703L) || (defined(__cplusplus) && __cplusplus >= 201703L)) && defined(__has_include) 38 | #if __has_include() && (!defined(__MAC_OS_X_VERSION_MIN_REQUIRED) || __MAC_OS_X_VERSION_MIN_REQUIRED >= 101500) 39 | #define GHC_USE_STD_FS 40 | #endif 41 | #endif 42 | #ifndef GHC_USE_STD_FS 43 | //#define GHC_WIN_DISABLE_WSTRING_STORAGE_TYPE 44 | #define GHC_FILESYSTEM_IMPLEMENTATION 45 | #include 46 | #endif 47 | -------------------------------------------------------------------------------- /3rdparty/jprogress_bar.hpp: -------------------------------------------------------------------------------- 1 | #ifndef JPROGRESS_BAR_HPP 2 | #define JPROGRESS_BAR_HPP 3 | 4 | /**************************************************** 5 | MIT License 6 | Copyright (c) 2022 ZhengqiaoWang 7 | Permission is hereby granted, free of charge, to any person obtaining a copy 8 | of this software and associated documentation files (the "Software"), to deal 9 | in the Software without restriction, including without limitation the rights 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | **/ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #if defined(_WIN32) 30 | #define WIN32_LEAN_AND_MEAN 31 | #define VC_EXTRALEAN 32 | #include 33 | #elif defined(__linux__) 34 | #include 35 | #endif 36 | 37 | namespace Joger 38 | { 39 | 40 | namespace ProgressBar 41 | { 42 | // If you want to diy the display style, you can edit every sign below 43 | static constexpr char START_SIGN{'['}; 44 | static constexpr char END_SIGN{']'}; 45 | static constexpr char PROGRESS_SIGN{'#'}; 46 | static constexpr char UNPROGRESS_SIGN{' '}; 47 | 48 | // Do not edit these variables if unnecessary 49 | static constexpr double PROGRESS_WIDTH_RATE{0.8}; 50 | static constexpr unsigned int PROGRESS_WORDS_WIDTH{10}; 51 | 52 | /** 53 | * @brief JProgressBar A simple progressbar for C++11 54 | * 55 | */ 56 | class JProgressBar 57 | { 58 | public: 59 | /** 60 | * @brief Construct a new JProgressBar object 61 | * 62 | * @param full_score The maximum value that will be written to the progress bar, 63 | * which can be the size of the file to be transferred. Requires greater than 0. 64 | */ 65 | JProgressBar(double full_score = 100.0,std::string pre_fix="FKF") : m_full_score(full_score) 66 | { 67 | assert(full_score > 0); 68 | getTerminalSize(); 69 | pre_fix_=pre_fix; 70 | m_full_point = (m_terminal_width - PROGRESS_WORDS_WIDTH) * PROGRESS_WIDTH_RATE; 71 | if(m_full_point>100){ 72 | m_full_point=100; 73 | // m_full_score=100; 74 | } 75 | } 76 | 77 | public: 78 | /** 79 | * @brief Update the progress 80 | * 81 | * @param cur_score current progress, which can be the size of the file already 82 | * transferred. Requires greater than or equal to 0. 83 | */ 84 | void update(double cur_score) 85 | { 86 | assert(cur_score <= m_full_score); 87 | assert(cur_score >= 0); 88 | m_cur_score = cur_score; 89 | m_cur_point = m_cur_score / m_full_score * m_full_point; 90 | printProgressBar(); 91 | } 92 | 93 | /** 94 | * @brief End progress bar, marks the end of the current progress bar. 95 | * 96 | */ 97 | void end() 98 | { 99 | printf("\n"); 100 | } 101 | 102 | private: 103 | void printProgressBar() 104 | { 105 | printf("\r"); 106 | printf("%s: %c", pre_fix_.c_str(),START_SIGN); 107 | for (unsigned int i = 0; i < m_full_point; ++i) 108 | { 109 | if (i <= m_cur_point) 110 | { 111 | printf("%c", PROGRESS_SIGN); 112 | } 113 | else 114 | { 115 | printf("%c", UNPROGRESS_SIGN); 116 | } 117 | } 118 | printf("%c %5.1f%%", END_SIGN, m_cur_score / m_full_score * 100); 119 | fflush(stdout); 120 | } 121 | 122 | void getTerminalSize() 123 | { 124 | #if defined(_WIN32) 125 | CONSOLE_SCREEN_BUFFER_INFO csbi; 126 | GetConsoleScreenBufferInfo(GetStdHandle(STD_OUTPUT_HANDLE), &csbi); 127 | m_terminal_width = (int)(csbi.dwSize.X); 128 | m_terminal_height = (int)(csbi.dwSize.Y); 129 | #elif defined(__linux__) 130 | struct winsize w; 131 | ioctl(fileno(stdout), TIOCGWINSZ, &w); 132 | m_terminal_width = (int)(w.ws_col); 133 | m_terminal_height = (int)(w.ws_row); 134 | #endif // Windows/Linux 135 | } 136 | 137 | private: 138 | int m_terminal_width{80}, m_terminal_height{10}; 139 | 140 | std::atomic m_cur_score{0}; 141 | double m_full_score; 142 | unsigned int m_full_point{0}; 143 | unsigned int m_cur_point{0}; 144 | std::string pre_fix_; 145 | }; 146 | } 147 | 148 | } 149 | 150 | #endif -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.0) 2 | cmake_policy(SET CMP0074 NEW) 3 | 4 | project(TGINS) 5 | message("Welcome to FUSING...") 6 | 7 | if(CMAKE_SYSTEM_NAME MATCHES "Linux") 8 | add_compile_options(-O3) 9 | add_compile_options(-pthread) 10 | add_compile_options(-w) 11 | add_compile_options(-m64) 12 | add_compile_options(-lz) 13 | add_compile_options(-lstdc++) 14 | elseif(CMAKE_SYSTEM_NAME MATCHES "Darwin") 15 | add_compile_options(-g) 16 | add_compile_options(-O3) 17 | add_compile_options(-Wall) 18 | add_compile_options(-pthread) 19 | endif() 20 | 21 | include(CheckCXXCompilerFlag) 22 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 23 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 24 | if(COMPILER_SUPPORTS_CXX11) 25 | set(CMAKE_CXX_STANDARD 11) 26 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 27 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -pthread") 28 | elseif(COMPILER_SUPPORTS_CXX0X) 29 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -pthread") 30 | else() 31 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 32 | endif() 33 | set(CMAKE_CXX_FLAGS "-std=c++11 -O0 -fPIC") 34 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 35 | set(CMAKE_CXX_FLAGS "-msse3") 36 | set(CMAKE_C_FLAGS "-msse3") 37 | set(CMAKE_C_FLAGS "-mavx") 38 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DELPP_NO_DEFAULT_LOG_FILE -DELPP_DISABLE_LOG_FILE_FROM_ARGs -DELPP_FRESH_LOG_FILE") 39 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DELPP_DISABLE_LOGS ") 40 | # DELPP_DISABLE_DEBUG_LOGS 41 | 42 | #Set the ROOT and subdirectory, you should put the CMakeList.txt in these file directories 43 | set(ROOT ${PROJECT_SOURCE_DIR}) 44 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${ROOT}/build/Bin) 45 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${ROOT}/build/Lib) 46 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${ROOT}/build/Lib) 47 | 48 | set(CMAKE_DEBUG_POSTFIX "d") 49 | set(CMAKE_RELEASE_POSTFIX "") 50 | set(CMAKE_RELWITHDEBINFO_POSTFIX "rd") 51 | set(CMAKE_MINSIZEREL_POSTFIX "s") 52 | 53 | #Choose different compilation configurations according to VS compilation 54 | if(CMAKE_BUILD_TYPE MATCHES "Release") 55 | set(CMAKE_BUILD_POSTFIX "${CMAKE_RELEASE_POSTFIX}") 56 | elseif(CMAKE_BUILD_TYPE MATCHES "Debug") 57 | set(CMAKE_BUILD_POSTFIX "${CMAKE_DEBUG_POSTFIX}") 58 | elseif(CMAKE_BUILD_TYPE MATCHES "RelWithDebInfo") 59 | set(CMAKE_BUILD_POSTFIX "${CMAKE_RELWITHDEBINFO_POSTFIX}") 60 | elseif(CMAKE_BUILD_TYPE MATCHES "MinSizeRel") 61 | set(CMAKE_BUILD_POSTFIX "${CMAKE_MINSIZEREL_POSTFIX}") 62 | else() 63 | set(CMAKE_BUILD_POSTFIX "") 64 | endif() 65 | 66 | # Eigen3 67 | find_package(Eigen3 REQUIRED) 68 | include_directories(${EIGEN3_INCLUDE_DIR}) 69 | 70 | # yaml-cpp 71 | find_package(yaml-cpp REQUIRED) 72 | 73 | set(CommonLib CommonLib) 74 | set(FImuLib FImuLib) 75 | set(FGnssLib FGnssLib) 76 | set(FCameraLib FCameraLib) 77 | set(FUwbLib FUwbLib) 78 | set(FLidarLib FLidarLib) 79 | set(FRadarLib FRadarLib) 80 | set(FusingLib FusingLib) 81 | set(MiscLib MiscLib) 82 | 83 | 84 | set(libCommonSrc ${PROJECT_SOURCE_DIR}/src/common) 85 | set(libFImuSrc ${PROJECT_SOURCE_DIR}/src/sensors/imu) 86 | set(libFGnssSrc ${PROJECT_SOURCE_DIR}/src/sensors/gnss) 87 | set(libFusingSrc ${PROJECT_SOURCE_DIR}/src/fusing) 88 | set(libMiscSrc ${PROJECT_SOURCE_DIR}/src/misc) 89 | 90 | add_subdirectory(${libCommonSrc} ${ROOT}/build/${CommonLib}) 91 | add_subdirectory(${libFImuSrc} ${ROOT}/build/${FImuLib}) 92 | add_subdirectory(${libFGnssSrc} ${ROOT}/build/${FGnssLib}) 93 | add_subdirectory(${libFusingSrc} ${ROOT}/build/${FusingLib}) 94 | add_subdirectory(${libMiscSrc} ${ROOT}/build/${MiscLib}) 95 | 96 | 97 | add_subdirectory(app/cui ${ROOT}/build/App) 98 | 99 | # group 100 | SET_PROPERTY(GLOBAL PROPERTY USE_FOLDERS ON) 101 | SET_PROPERTY(TARGET ${CommonLib} PROPERTY FOLDER "LIB") 102 | SET_PROPERTY(TARGET ${FImuLib} PROPERTY FOLDER "LIB") 103 | SET_PROPERTY(TARGET ${FGnssLib} PROPERTY FOLDER "LIB") 104 | SET_PROPERTY(TARGET ${MiscLib} PROPERTY FOLDER "LIB") 105 | 106 | #Output Messages for debug the Cmake 107 | message(STATUS "operation system is : ${CMAKE_SYSTEM}") 108 | message(STATUS "current platform is : ${CMAKE_SYSTEM_NAME}") 109 | message(STATUS "CMake version is : ${CMAKE_SYSTEM_VERSION}") 110 | message(STATUS "C compiler is : ${CMAKE_C_COMPILER}") 111 | message(STATUS "C++ compiler is : ${CMAKE_CXX_COMPILER}") 112 | message(STATUS "The program main directory is : ${ROOT}") 113 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # TGINS: Tightly couple GNSS/INS system,基于[RTKLIB](https://github.com/tomojitakasu/RTKLIB.git)修改的GNSS/INS紧组合代码 2 | 3 | ## 一. 支持功能 4 | 1. 继承[RTKLIB-demo5](https://github.com/rtklibexplorer/RTKLIB) 5 | 2. 支持GNSS/INS松组合 6 | 3. 支持GNSS/INS松组合RTS 7 | 4. 支持GPS PPK/INS半紧组合、紧组合 8 | 5. 支持GPS PPP/INS紧组合 9 | 6. **支持NED-FRD、ENU-RFU姿态定义的GNSS/INS松、紧组合** 10 | 7. 结果\误差可视化（FUSING-PLOT） 11 | 12 | ## 二. 结果示例 13 | 1. PPK/INS松组合（loosely coupled）
14 | 以[i2NAV](https://github.com/i2Nav-WHU/awesome-gins-datasets)公开数据集(ADIS16465)为例 15 |
16 | 20 | 24 | 28 |
29 |
ADIS16465松组合-位置、速度、姿态误差
33 |
34 |
35 | 39 | 43 |
44 |
ADIS16465松组合-陀螺、加计零偏
48 |
49 |
50 | 54 |
55 |
ADIS16465松组合-误差统计
59 |
60 |
61 | 64 |
65 | 以2019-100C-北京数据集为例，IE后处理平滑结果作为参考
66 |
67 | 71 | 75 | 79 |
80 |
100C松组合-位置、速度、姿态误差
84 |
85 |
86 | 90 | 94 |
95 |
100C松组合-陀螺、加计零偏
99 |
100 |
101 | 105 |
106 |
100C松组合-误差统计
110 |
111 |
112 | 115 |
116 | 3. PPK/INS半紧组合 （semi-tightly coupled）
117 | 以2019-100C-北京数据集为例，IE后处理平滑结果作为参考 118 |
119 | 123 | 127 | 131 |
132 |
100C半紧组合-位置、速度、姿态误差
136 |
137 |
138 | 142 |
143 |
100C半紧组合-误差统计
147 |
148 |
149 | 152 |
153 | 154 | 4. PPK/INS紧组合 （tightly coupled）
155 | 以2019-100C-北京数据集为例 156 |
157 | 161 | 165 | 169 |
170 |
100C PPK/INS紧组合-位置、速度、姿态误差
174 |
175 |
176 | 180 |
181 |
100C PPK/INS紧组合-误差统计
185 |
186 |
187 | 190 |
191 | 192 | 5. PPP/INS紧组合 （tightly coupled）
193 | 以2019-100C-北京数据集为例 194 |
195 | 199 | 203 | 207 |
208 |
100C PPP/INS紧组合-位置、速度、姿态误差
212 |
213 |
214 | 218 |
219 |
100C PPP/INS紧组合-误差统计
223 |
224 |
225 | 228 |
229 | 230 | ## 三. 编译依赖
231 | 1. YAML(>=0.6) 232 | 2. Eigen(>=3.40) 233 | 3. 调试建议在ubuntu使用CLION，FUSING-PLOT运行可能需要依赖QT(>=5.15) 234 | 235 | ## 四. 数据准备
236 | 1. 可在配置文件中手动配置所需要的文件路径 237 | 2. 若自动匹配所需要的文件，则需要按一定结构存储所需要的观测文件和产品文件，只需在配置文件中配置主文件路径即可 238 |
239 | 243 |
244 |
主文件夹示例
248 |
249 |
250 | 254 |
255 |
year文件夹示例
259 |
260 |
261 | 265 |
266 |
doy文件夹示例
270 |
271 |
272 | 276 |
277 |
obs文件夹示例
281 |
282 |
283 | 287 |
288 |
products文件夹示例
292 |
293 | 294 | ## 五. 存在的问题 295 | - [x] PPP/INS紧组合：ppp/ins紧组合算法已实现，可运行，基于rtklib的ppp算法需进一步的优化 296 | - [x] 多星座GNSS：仅测试了单GPS，后续会支持多星座GNSS 297 | - [x] 算法鲁棒性： 仅测试了两组数据验证算法的稳定性，后续会增加测试不同场景数据 298 | - [x] 完善文档说明 299 | - [x] ... 300 | 301 | ## 六. 说明 302 | - [x] 代码质量和结构仍存在漏洞，仅作参考。 303 | - [x] 代码会陆续更新，但需要一定时间，直至完全支持多星座多频PPP/INS、PPK/INS紧组合 304 | - [x] 希望能做成一个有意义的项目，欢迎大家有数据的能够投喂，双天线/GNSS/INS/UWB/ODO/CAMERA/LiDAR/手机等、车载、机载、船载等,后续会继续上传一些车载GNSS/INS紧组合数据 305 | - [x] 作者：黑娃（chenchaochn@163.com，安徽理工大学空间信息与测绘工程学院导航工程系） -------------------------------------------------------------------------------- /app/cui/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.0) 2 | cmake_policy(SET CMP0074 NEW) 3 | 4 | project(FUSING-APP) 5 | 6 | include_directories(${libCommonSrc} 7 | ${libFImuSrc} 8 | ${libFGnssSrc} 9 | ${libFusingSrc} 10 | ${libMiscSrc}) 11 | 12 | 13 | add_executable(TGINS TGINS.cc) 14 | target_link_libraries(TGINS ${CommonLib} ${FGnssLib} ${FImuLib} ${FusingLib} ${MiscLib}) 15 | 16 | add_executable(compareSol compareSol.cc) 17 | target_link_libraries(compareSol ${CommonLib} ${FGnssLib} ${FImuLib} ${MiscLib}) 18 | 19 | -------------------------------------------------------------------------------- /app/cui/TGINS.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 10/2/22. 3 | // 4 | 5 | #include "cxxopts.hpp" 6 | #include "Config.h" 7 | #include "Gnss.h" 8 | #include "rtklib/rtklib.h" 9 | #include "Fusing.h" 10 | #include "MatchFiles.h" 11 | #include "ProcessBar.h" 12 | 13 | // trace=2,debug=4,fatal=8, error=16, warning=32, verbose=64, info=128 14 | int main(int argc,char **argv) 15 | { 16 | long t1,t2; 17 | t1=clock(); 18 | 19 | cxxopts::Options options("Fusing","-c config_file"); 20 | options.add_options() 21 | ("c,config","yaml-format configuration file", cxxopts::value()) 22 | ("l,logc", "log system configuration file path",cxxopts::value()) 23 | ("f,format","reference solution format",cxxopts::value()) 24 | ("d,debug", "debug level(T2,D4,F8,E16,W32,V64,I128)",cxxopts::value()) 25 | ; 26 | 27 | auto result=options.parse(argc,argv); 28 | 29 | if(result.count("help")){ 30 | cout<(); 37 | } 38 | else{ 39 | cout<()).c_str()); 46 | } 47 | 48 | if(result.count("logc")){ 49 | logc_file=result["logc"].as(); 50 | } 51 | 52 | int log_level=8; 53 | if(result.count("debug")){ 54 | log_level=result["debug"].as(); 55 | } 56 | 57 | /*initialize logger system*/ 58 | initLogger(argc,argv,logc_file,log_level); 59 | 60 | LOG(INFO)<<"Welcome to TGINS"; 61 | 62 | Config config; 63 | if(!config.parse(conf_file)){ 64 | LOG(ERROR)<<"msf: parse configuration file error"; 65 | return -1; 66 | } 67 | 68 | fusing_t fusing; 69 | fusingMeas_t meas={0}; 70 | 71 | /*initialize FUSING system*/ 72 | initFusing(fusing,config); 73 | 74 | if(!checkInputFiles(fusing,fusing.opts,log_level)){ 75 | LOG(INFO)<<"TGINS: miss necessary files, exit ..."; 76 | return -1; 77 | } 78 | 79 | if(!fusing.opts.input.log_files.empty()){ 80 | setLogFile(fusing.opts.input.log_files,log_level); 81 | } 82 | 83 | /*load required data*/ 84 | if(!loadMeasurement(fusing,fusing.opts,meas)){ 85 | LOG(ERROR)<<"TGINS: load measurement failed, exit..."; 86 | return -1; 87 | } 88 | 89 | /*multi sensor fusion*/ 90 | if(!multiSensorFusing(meas,fusing)){ 91 | LOG(ERROR)<<"TGINS: GNSS/INS fusion process failed, exit..."; 92 | return -1; 93 | } 94 | 95 | /*compare solutions*/ 96 | if(!fusing.opts.input.ref_file.empty()&&!fusing.opts.input.err_file.empty()){ 97 | char s[128]; 98 | sprintf(s,"%s -c %s -d %d -f %s" ,"/home/hw/Workspace/NewLab/Fusing/build/Bin/compareSol",conf_file.c_str(),log_level,(result["format"].as()).c_str()); 99 | cout< 9 | #include 10 | #include "Enums.h" 11 | #include "EigenInc.h" 12 | #include "GTime.h" 13 | #include "rtklib.h" 14 | 15 | using namespace std; 16 | 17 | struct imuProperty_t; 18 | struct prcopt_t; 19 | struct solopt_t; 20 | 21 | typedef struct { 22 | string prc_dir; 23 | vector rnx_files; 24 | vector nav_files; 25 | vector sp3_files; 26 | vector clk_files; 27 | 28 | vector erp_files; 29 | vector obx_files; 30 | vector bia_files; 31 | vector dcb_files; 32 | string bsx_file; 33 | string blq_file; 34 | string ion_file; 35 | string atx_file; 36 | string snx_file; 37 | 38 | string egm_file; 39 | string jpl_file; 40 | 41 | string imu_file; 42 | string imup_file; 43 | string pos_file; 44 | string sol_file; 45 | string ref_file; 46 | string err_file; 47 | string trace_file; 48 | string solstat_file; 49 | vector log_files; 50 | }inputopt_t; 51 | 52 | typedef struct { 53 | double sow_start=0.0,sow_end=0.0; 54 | E_PrcMode prc_mode=E_PrcMode::INS; 55 | E_Estimator filter_type=E_Estimator::FKF; 56 | vector prc_date; 57 | string prc_site; 58 | }commonopt_t; 59 | 60 | typedef struct { 61 | double ts; 62 | int n; 63 | double spacing; 64 | vector duration; 65 | }simGpsLoss_t; 66 | 67 | typedef struct{ 68 | E_InsAlign align_method = E_InsAlign::MANUAL; 69 | E_InsNavCoord nav_coord = E_InsNavCoord::LLH; 70 | E_AttDefination att_def = E_AttDefination::ENU_RFU; 71 | E_GnssPvFmt gnsspv_fmt = E_GnssPvFmt::RTKLIB; 72 | simGpsLoss_t sim_gps_loss = {0}; 73 | E_SwitchOpt est_Sa=E_SwitchOpt::OFF,est_Sg=E_SwitchOpt::OFF; 74 | E_SwitchOpt dop_aid=E_SwitchOpt::OFF; 75 | E_SwitchOpt tdcp_aid=E_SwitchOpt::OFF; 76 | E_SwitchOpt zupt_aid=E_SwitchOpt::OFF; 77 | E_SwitchOpt nhc_aid=E_SwitchOpt::OFF; 78 | }imuopt_t; 79 | 80 | typedef struct { 81 | double sample_rate=30.0; 82 | E_GnssMode mode=E_GnssMode::KINEMATIC; 83 | string freq="l1+l2"; 84 | int nf=2; 85 | double el_mask=15.0*D2R; 86 | E_SwitchOpt snrmask[2]={E_SwitchOpt::OFF,E_SwitchOpt::OFF}; 87 | vector snrmask_L[NFREQ]; 88 | E_SwitchOpt dynamics = E_SwitchOpt::OFF; 89 | E_TideCorr tide_corr = E_TideCorr::OFF; 90 | E_IonoOpt iono_opt = E_IonoOpt::BRDC; 91 | E_TropOpt trop_opt = E_TropOpt::SAAS; 92 | E_SatEph sat_eph = E_SatEph::BRDC; 93 | string pre_ac; 94 | 95 | E_SwitchOpt sat_pcv = E_SwitchOpt::OFF; 96 | E_SwitchOpt rec_pcv = E_SwitchOpt::OFF; 97 | E_SwitchOpt phw = E_SwitchOpt::OFF; 98 | E_SwitchOpt eclipsing = E_SwitchOpt::OFF; 99 | E_SwitchOpt raim = E_SwitchOpt::OFF; 100 | E_SwitchOpt clkjump = E_SwitchOpt::OFF; 101 | E_SwitchOpt sd_gnss = E_SwitchOpt::OFF; 102 | 103 | int excl_sats[MAXSAT]={0}; 104 | int nav_sys; 105 | 106 | E_ARMode ar_mode = E_ARMode::CONTINUOUS; 107 | E_SwitchOpt glo_ar_mode = E_SwitchOpt::OFF; 108 | E_SwitchOpt bds_ar_mode = E_SwitchOpt::OFF; 109 | E_SwitchOpt ar_filter = E_SwitchOpt::OFF; 110 | vector arthres = {3.0,0.1, 0.0, 1E-09, 1E-05}; 111 | double ratio_min = 2.5; 112 | double ratio_max = 3.0; 113 | double varholdmab = 0.1; 114 | double gainholdamb = 0.01; 115 | int arlock_cnt = 10; 116 | int minfix_sats = 4; 117 | int minhold_sats = 5; 118 | int mindrop_sats = 10; 119 | int armin_fix = 20; 120 | int armax_iter = 1; 121 | double elmask_ar = 0.0; 122 | double elmask_hold = 0.0; 123 | int arout_cnt = 20; 124 | 125 | double max_age = 30; 126 | E_SwitchOpt sync_sol = E_SwitchOpt::OFF; 127 | double slip_thres = 0.05; 128 | double rej_inno[2] = {30,30}; 129 | double rej_dop = 30; 130 | int niter = 1; 131 | double base_len = 0.0; 132 | double base_sig = 0.0; 133 | 134 | E_WeightMode weight_mode=E_WeightMode::ELEVATION; 135 | vector eratio = {100,100,100}; 136 | double err_phase = 0.003; 137 | double err_phase_el = 0.003; 138 | double err_phase_bl = 0.0; 139 | double err_doppler = 1.0; 140 | double snr_max = 52.0; 141 | double err_snr = 0.0; 142 | double err_rcv = 0.0; 143 | double std_bias = 30.0; 144 | double std_iono = 0.03; 145 | double std_trop = 0.3; 146 | double prn_accelh = 3.0; 147 | double prn_accelv = 1.0; 148 | double prn_bias = 0.0; 149 | double prn_iono = 0.001; 150 | double prn_trop = 0.0001; 151 | double prn_pos = 0.0; 152 | double clk_stab = 5e-12; 153 | 154 | E_PosFmt ant_pos_fmt[2] = {E_PosFmt::LLH,E_PosFmt::LLH}; 155 | Vector3d ant_pos[2] = {Vector3d::Zero(),Vector3d::Zero()}; 156 | string ant_type[2]; 157 | Vector3d ant_del[2] = {Vector3d::Zero(),Vector3d::Zero()}; 158 | double maxaveep = 0.0; 159 | E_SwitchOpt initrst = E_SwitchOpt::OFF; 160 | }gnssopt_t; 161 | 162 | typedef struct { 163 | 164 | }cameraopt_t; 165 | 166 | typedef struct { 167 | E_SolFmt sol_fmt = E_SolFmt::LLH; 168 | E_SwitchOpt out_head = E_SwitchOpt::OFF; 169 | E_SwitchOpt out_opt = E_SwitchOpt::OFF; 170 | E_SwitchOpt out_vel = E_SwitchOpt::OFF; 171 | E_SwitchOpt out_ins = E_SwitchOpt::OFF; 172 | E_TimeSys time_sys = E_TimeSys::GPST; 173 | E_TimeFmt time_fmt = E_TimeFmt::TOW; 174 | int time_ndec = 3; 175 | string field_sep; 176 | E_DegFmt deg_fmt = E_DegFmt::DEG; 177 | E_SwitchOpt out_single = E_SwitchOpt::OFF; 178 | double max_sol_std = 0; 179 | E_HeightFmt height = E_HeightFmt::ELLIPSOIDAL; 180 | double nmea_intv1 = 0; 181 | double nmea_intv2 = 0; 182 | E_SolStat out_sol_stat = E_SolStat::OFF; 183 | int trace; 184 | }outputopt_t; 185 | 186 | typedef struct { 187 | prcopt_t prc_opt=prcopt_default; 188 | solopt_t sol_opt=solopt_default; 189 | }rtklibopt_t; 190 | 191 | typedef struct{ 192 | commonopt_t common; 193 | imuopt_t imu; 194 | gnssopt_t gnss; 195 | cameraopt_t camera; 196 | inputopt_t input; 197 | outputopt_t output; 198 | rtklibopt_t rtklib; 199 | }fusingopt_t; 200 | 201 | class Config { 202 | public: 203 | Config()=default; 204 | Config(string file_path); 205 | ~Config()=default; 206 | 207 | private: 208 | bool parse(); 209 | 210 | public: 211 | bool parse(string file_path); 212 | void getRtklibOpts(prcopt_t *popt,solopt_t *sopt); 213 | static bool parseImuConf(string file_path,imuProperty_t& imuc,E_AttDefination att_def); 214 | 215 | private: 216 | string config_file_; 217 | YAML::Node yaml_; 218 | 219 | public: 220 | fusingopt_t fusingopt_; 221 | }; 222 | 223 | 224 | #endif //FUSING_CONFIG_H 225 | -------------------------------------------------------------------------------- /src/common/Constants.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 8/27/22. 3 | // 4 | 5 | #ifndef FUSING_CONSTANTS_H 6 | #define FUSING_CONSTANTS_H 7 | 8 | /*imu related*/ 9 | 10 | #define D2R (M_PI/180.0) /* deg to rad */ 11 | #define R2D (180.0/M_PI) /* rad to deg */ 12 | #define DPH2RPS 4.84813681109536e-06 /* deg/h to rad/s */ 13 | #define RPS2DPH 206264.806247096 /* rad/s to deg/h */ 14 | #define DPSH2RPSS 2.90888208665722e-4 /* deg/sqrt(h) to rad/sqrt(s) */ 15 | #define RPSS2DPSH 3437.74677078493 /* rad/sqrt(s) to deg/sqrt(h) */ 16 | #define DPS2DPH 3600.0 17 | #define DPHPSHZ2RPSS 4.84813681109536e-06 /* deg/h/sqrt(Hz) to rad/sqrt(s) */ 18 | #define RPSS2DPHPSHZ 206264.806247096 /* rad/sqrt(s) to deg/h/sqrt(Hz) */ 19 | #define G2MPS2 9.7803267714 /* g0 to m/s^2 */ 20 | #define MPS22G 0.101971621297793 /* m/s^2 to g0 */ 21 | #define MG2MPS2 9.7803267714e-3 /* millo-g0(mg) to m/s^2 */ 22 | #define UG2MPS2 9.7803267714e-6 23 | #define MPS22MG 101.971621297793 /* m/s^2 to milli-g0(mg) */ 24 | #define MPS22UG 101971.621297793 /* m/s^2 to micro-g0(ug) */ 25 | #define MPSPSH2MPSSS 0.01666666666667 26 | #define MPSSS2MPSPSH 60.0 27 | #define GAL2MPS2 0.01 /* gal to m/s^2 */ 28 | #define MPS22GAL 100.0 /* m/s^2 to gal */ 29 | #define MGAL2MPS2 1E-5 /* mGal to m/s^2 */ 30 | #define MPS22MGAL 100000 /* m/s^2 to mGal */ 31 | #define SCALE2PPM 1E6 32 | #define PPM2SCALE 1E-6 33 | 34 | #define WGS84_OMGE 7.2921151467E-5 35 | #define WGS84_RE 6378137.0 36 | #define WGS84_RP 6356752.31425 37 | #define WGS84_MU 3.986004418E14 38 | #define WGS84_J2 1.082627E-3 39 | #define WGS84_FE (1.0/298.257223563) 40 | #define WGS84_ECC 0.081819221455523 41 | #define WGS84_ECC2 0.006694384999588 42 | #define GRAVITY0 9.7803267714 43 | 44 | /*gnss related*/ 45 | #define JD2MJD 2400000.5 /* JD to MJD */ 46 | 47 | #define MAXLEAPS 64 /* max number of leap seconds table */ 48 | 49 | 50 | #endif //FUSING_CONSTANTS_H 51 | -------------------------------------------------------------------------------- /src/common/Coordinate.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 8/27/22. 3 | // 4 | 5 | #include "Coordinate.h" 6 | #include "Constants.h" 7 | #include "Rotation.h" 8 | 9 | 10 | Vector3d Coordinate::ecef2llh(Eigen::Vector3d re) { 11 | double e2=WGS84_FE*(2.0-WGS84_FE),r2=re[0]*re[0]+re[1]*re[1],z,zk,v=WGS84_RE,sinp; 12 | 13 | for (z=re[2],zk=0.0;fabs(z-zk)>=1E-4;) { 14 | zk=z; 15 | sinp=z/sqrt(r2+z*z); 16 | v=WGS84_RE/sqrt(1.0-e2*sinp*sinp); 17 | z=re[2]+v*e2*sinp; 18 | } 19 | Vector3d llh; 20 | llh[0]=r2>1E-12?atan(z/sqrt(r2)):(re[2]>0.0?M_PI/2.0:-M_PI/2.0); 21 | llh[1]=r2>1E-12?atan2(re[1],re[0]):0.0; 22 | llh[2]=sqrt(r2+z*z)-v; 23 | 24 | return llh; 25 | } 26 | 27 | Vector3d Coordinate::llh2ecef(Eigen::Vector3d llh) { 28 | double sinp=sin(llh[0]),cosp=cos(llh[0]),sinl=sin(llh[1]),cosl=cos(llh[1]); 29 | double e2=WGS84_FE*(2.0-WGS84_FE),v=WGS84_RE/sqrt(1.0-e2*sinp*sinp); 30 | 31 | Vector3d re; 32 | re[0]=(v+llh[2])*cosp*cosl; 33 | re[1]=(v+llh[2])*cosp*sinl; 34 | re[2]=(v*(1.0-e2)+llh[2])*sinp; 35 | 36 | return re; 37 | } 38 | 39 | Vector3d Coordinate::ecef2local(Eigen::Vector3d llh, Eigen::Vector3d re, E_AttDefination llh_type) { 40 | Matrix3d Cne=Rotation::getCne(llh,llh_type); 41 | return Cne.transpose()*re; 42 | } 43 | 44 | Vector3d Coordinate::local2ecef(Eigen::Vector3d llh, Eigen::Vector3d rn, E_AttDefination llh_type) { 45 | Matrix3d Cne=Rotation::getCne(llh,llh_type); 46 | return Cne*rn; 47 | } -------------------------------------------------------------------------------- /src/common/Coordinate.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 8/27/22. 3 | // 4 | 5 | #ifndef FUSING_COORDINATE_H 6 | #define FUSING_COORDINATE_H 7 | 8 | #include "EigenInc.h" 9 | #include "Enums.h" 10 | 11 | 12 | class Coordinate { 13 | public: 14 | Coordinate() = default; 15 | ~Coordinate() = default; 16 | 17 | public: 18 | static Vector3d ecef2llh(Vector3d re); 19 | static Vector3d llh2ecef(Vector3d llh); 20 | static Vector3d ecef2local(Vector3d llh,Vector3d re,E_AttDefination llh_type); 21 | static Vector3d local2ecef(Vector3d llh,Vector3d rn,E_AttDefination llh_type); 22 | }; 23 | 24 | 25 | #endif //FUSING_COORDINATE_H 26 | -------------------------------------------------------------------------------- /src/common/EigenInc.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 8/29/22. 3 | // 4 | 5 | #ifndef FUSING_EIGENINC_H 6 | #define FUSING_EIGENINC_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using Eigen::SimplicialLLT; 16 | using Eigen::SimplicialLDLT; 17 | using Eigen::COLAMDOrdering; 18 | using Eigen::LDLT; 19 | using Eigen::LLT; 20 | using Eigen::PartialPivLU; 21 | using Eigen::Matrix; 22 | using Eigen::MatrixXd; 23 | using Eigen::Matrix2d; 24 | using Eigen::Matrix3d; 25 | using Eigen::VectorXd; 26 | using Eigen::Vector3d; 27 | using Eigen::Vector2d; 28 | using Eigen::MatrixXi; 29 | using Eigen::SparseMatrix; 30 | using Eigen::SparseVector; 31 | using Eigen::SparseQR; 32 | using Eigen::Map; 33 | using Eigen::Quaterniond; 34 | using Eigen::Triplet; 35 | using Eigen::ArrayXd; 36 | typedef Eigen::Array ArrayXb; 37 | #define V30 Eigen::Vector3d::Zero() 38 | #define M30 Eigen::Matrix3d::Zero() 39 | #endif //FUSING_EIGENINC_H 40 | -------------------------------------------------------------------------------- /src/common/Enums.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 8/27/22. 3 | // 4 | 5 | #ifndef FUSING_ENUMS_H 6 | #define FUSING_ENUMS_H 7 | 8 | #define BETTER_ENUMS_DEFAULT_CONSTRUCTOR(Enum) \ 9 | public: \ 10 | Enum() = default; 11 | #include "../../3rdparty/enum.h" 12 | 13 | BETTER_ENUM(E_SwitchOpt, short int, 14 | OFF, 15 | ON) 16 | 17 | BETTER_ENUM(E_FileType, short int, 18 | TEXT, 19 | BINARY) 20 | 21 | BETTER_ENUM(E_Estimator, short int, 22 | FKF, 23 | BKF, 24 | FBS, 25 | RTS, 26 | FGO) 27 | 28 | BETTER_ENUM(E_ImuFmt, short int, 29 | DEFAULT, 30 | NOVATEL, 31 | M39, 32 | PSINS, 33 | GINS, 34 | A15) 35 | 36 | BETTER_ENUM(E_AttDefination, short int, 37 | NED_FRD, 38 | ENU_RFU) 39 | 40 | BETTER_ENUM(E_InsNavCoord, short int, 41 | ECEF, 42 | LLH) 43 | 44 | BETTER_ENUM(E_InsAlign, short int, 45 | MANUAL, 46 | MOVING, 47 | GPSSOL, 48 | GPSOBS, 49 | DUALANT) 50 | 51 | BETTER_ENUM(E_GnssPvFmt, short int, 52 | PSINS, 53 | RTKLIB, 54 | GINS) 55 | 56 | BETTER_ENUM(E_RefSolFmt, short int, 57 | IE, 58 | PSINS, 59 | RTKLIB, 60 | GINS, 61 | FUSING) 62 | 63 | BETTER_ENUM(E_PrcMode, short int, 64 | GNSS, 65 | INS, 66 | IGLC, 67 | IGSTC, 68 | IGTC, 69 | VO, 70 | VIO, 71 | GVIO) 72 | 73 | BETTER_ENUM(E_GnssMode, short int, 74 | SOL, 75 | SINGLE, 76 | DGPS, 77 | KINEMATIC, 78 | STATIC, 79 | STATIC_START, 80 | MOVED, 81 | FIXED, 82 | PPP_KINEMA, 83 | PPP_STATIC, 84 | PPP_FIXED) 85 | 86 | BETTER_ENUM(E_TideCorr, short int, 87 | OFF, 88 | ON, 89 | OTL) 90 | 91 | BETTER_ENUM(E_IonoOpt, short int, 92 | OFF, 93 | BRDC, 94 | SBAS, 95 | IF, 96 | UC, 97 | GIM) 98 | 99 | BETTER_ENUM(E_TropOpt, short int, 100 | OFF, 101 | SAAS, 102 | SBAS, 103 | EST_ZTD, 104 | EST_ZTD_GRAD) 105 | 106 | BETTER_ENUM(E_SatEph, short int, 107 | BRDC, 108 | PRECISE, 109 | BRDC_SBAS, 110 | BRDC_SSRAPC, 111 | BRDC_SSRCOM) 112 | 113 | BETTER_ENUM(E_ARMode, short int, 114 | OFF, 115 | CONTINUOUS, 116 | INSTANTANEOUS, 117 | FIX_AND_HOLD) 118 | 119 | BETTER_ENUM(E_PosFmt, short int, 120 | LLH, 121 | XYZ, 122 | SPP, 123 | POS_FILE, 124 | RINEX_HEAD, 125 | RTCM, 126 | RAW) 127 | 128 | BETTER_ENUM(E_SolFmt, short int, 129 | LLH, 130 | XYZ, 131 | ENU, 132 | NMEA) 133 | 134 | BETTER_ENUM(E_TimeSys, short int, 135 | GPST, 136 | UTC, 137 | JST) 138 | 139 | BETTER_ENUM(E_TimeFmt, short int, 140 | TOW, 141 | HMS) 142 | 143 | BETTER_ENUM(E_DegFmt, short int, 144 | DEG, 145 | DMS) 146 | 147 | BETTER_ENUM(E_HeightFmt, short int, 148 | ELLIPSOIDAL, 149 | GEODETIC) 150 | 151 | BETTER_ENUM(E_SolStat, short int, 152 | OFF, 153 | STATE, 154 | RESIDUAL) 155 | 156 | BETTER_ENUM(E_WeightMode, short int, 157 | ELEVATION, 158 | SNR) 159 | #endif //FUSING_ENUMS_H 160 | -------------------------------------------------------------------------------- /src/common/GTime.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 8/27/22. 3 | // 4 | 5 | #include 6 | #include "GTime.h" 7 | 8 | 9 | const double gpst0[] = {1980, 1, 6, 0, 0, 0}; /* gps time reference */ 10 | const double gst0[] = {1999, 8, 22, 0, 0, 0}; /* galileo system time reference */ 11 | const double bdt0[] = {2006, 1, 1, 0, 0, 0}; /* beidou time reference */ 12 | 13 | const double leaps[MAXLEAPS + 1][7] = 14 | { 15 | /* leap seconds (y,m,d,h,m,s,utc-gpst) */ 16 | {2017, 1, 1, 0, 0, 0, -18}, 17 | {2015, 7, 1, 0, 0, 0, -17}, 18 | {2012, 7, 1, 0, 0, 0, -16}, 19 | {2009, 1, 1, 0, 0, 0, -15}, 20 | {2006, 1, 1, 0, 0, 0, -14}, 21 | {1999, 1, 1, 0, 0, 0, -13}, 22 | {1997, 7, 1, 0, 0, 0, -12}, 23 | {1996, 1, 1, 0, 0, 0, -11}, 24 | {1994, 7, 1, 0, 0, 0, -10}, 25 | {1993, 7, 1, 0, 0, 0, -9}, 26 | {1992, 7, 1, 0, 0, 0, -8}, 27 | {1991, 1, 1, 0, 0, 0, -7}, 28 | {1990, 1, 1, 0, 0, 0, -6}, 29 | {1988, 1, 1, 0, 0, 0, -5}, 30 | {1985, 7, 1, 0, 0, 0, -4}, 31 | {1983, 7, 1, 0, 0, 0, -3}, 32 | {1982, 7, 1, 0, 0, 0, -2}, 33 | {1981, 7, 1, 0, 0, 0, -1}, 34 | {0} 35 | }; 36 | 37 | GTime GTime::yrdoy2time(int year,int doy) { 38 | double ep[6]={ 0 }; 39 | ep[0] = year; ep[1] = 1; ep[2] = 1; 40 | ep[3] = 0; ep[4] = 0; ep[5] = 0; 41 | GTime t_jan1 = GTime::ymdhms2time(ep); 42 | double dt = (doy-1)*86400.0; 43 | GTime tt = t_jan1+dt; 44 | 45 | return tt; 46 | } 47 | 48 | int GTime::time2yrdoy(GTime t2, int *year) { 49 | double ep[6]={0}; 50 | GTime::time2ymdhms(t2, ep); 51 | if (year) *year = (int)ep[0]; 52 | 53 | /* time at yyyy-01-01 00:00:00 */ 54 | ep[1] = 1; ep[2] = 1; ep[3] = 0; ep[4] = 0; ep[5] = 0; 55 | GTime t_jan1 = GTime::ymdhms2time(ep); 56 | 57 | double dt = t2 - t_jan1; 58 | int doy=0; 59 | doy = (int)(dt/86400.0 + 1.0e-9) + 1; 60 | return doy; 61 | } 62 | 63 | GTime GTime::ymdhms2time(const double *ep) { 64 | const int doy[] = {1,32,60,91,121,152,182,213,244,274,305,335}; 65 | GTime tt={}; 66 | 67 | int year=(int)ep[0]; 68 | int mon=(int)ep[1]; 69 | int day=(int)ep[2]; 70 | 71 | if(year<1970||year>2099||mon<1||mon>12){ 72 | return tt; 73 | } 74 | 75 | int days = (year-1970)*365+(year-1969)/4+doy[mon-1]+day-2+(year%4==0&&mon>=3?1:0); 76 | int sec = (int) floor(ep[5]); 77 | tt.t_.time = (time_t)days*86400+(int)ep[3]*3600+(int)ep[4]*60+sec; 78 | tt.t_.sec = ep[5] - sec; 79 | 80 | return tt; 81 | } 82 | 83 | void GTime::time2ymdhms(const GTime t2,double *ep) { 84 | const int mday[]= 85 | { 86 | 31,28,31,30,31,30,31,31,30,31,30,31, 87 | 31,28,31,30,31,30,31,31,30,31,30,31, 88 | 31,29,31,30,31,30,31,31,30,31,30,31, 89 | 31,28,31,30,31,30,31,31,30,31,30,31 90 | }; 91 | 92 | int days = (int) (t2.t_.time / 86400); 93 | int sec = (int) (t2.t_.time - (time_t) days * 86400); 94 | 95 | int doy = days % (365*4+1); 96 | int mon; 97 | for (mon = 0; mon < 48; mon++) 98 | { 99 | if (doy >= mday[mon]) 100 | doy -= mday[mon]; 101 | else 102 | break; 103 | } 104 | ep[0] = 1970 + days / 1461 * 4 + mon / 12; 105 | ep[1] = mon % 12 + 1; 106 | ep[2] = doy + 1; 107 | ep[3] = sec / 3600; 108 | ep[4] = sec % 3600 / 60; 109 | ep[5] = sec % 60 + t2.t_.sec; 110 | } 111 | 112 | GTime GTime::gpst2utc(){ 113 | int i; 114 | for(i=0;leaps[i][0]>0;i++){ 115 | GTime tu=*this+leaps[i][6]; 116 | if((tu-ymdhms2time(leaps[i]))>=0){ 117 | return tu; 118 | } 119 | } 120 | return *this; 121 | } 122 | 123 | GTime GTime::utc2gpst() { 124 | int i; 125 | for(i=0;leaps[i][0]>0;i++){ 126 | if((*this-ymdhms2time(leaps[i]))>=0){ 127 | return *this-leaps[i][6]; 128 | } 129 | } 130 | return *this; 131 | } 132 | 133 | GTime GTime::gpst2bdt() { 134 | return *this-14.0; 135 | } 136 | 137 | GTime GTime::bdt2gpst() { 138 | return *this+14.0; 139 | } 140 | 141 | GTime GTime::gpst2utc(GTime t2){ 142 | int i; 143 | for(i=0;leaps[i][0]>0;i++){ 144 | GTime tu=t2+leaps[i][6]; 145 | if((tu-ymdhms2time(leaps[i]))>=0){ 146 | return tu; 147 | } 148 | } 149 | return t2; 150 | } 151 | 152 | GTime GTime::utc2gpst(GTime t2) { 153 | int i; 154 | for(i=0;leaps[i][0]>0;i++){ 155 | if((t2-ymdhms2time(leaps[i]))>=0){ 156 | return t2-leaps[i][6]; 157 | } 158 | } 159 | return t2; 160 | } 161 | 162 | GTime GTime::gpst2bdt(GTime t2) { 163 | return t2-14.0; 164 | } 165 | 166 | GTime GTime::bdt2gpst(GTime t2) { 167 | return t2+14.0; 168 | } 169 | 170 | GTime GTime::gpst2time(int week, double sec) { 171 | GTime t2= GTime::ymdhms2time(gpst0); 172 | if(sec<-1E9||sec>1E9){ 173 | sec=0; 174 | } 175 | t2.t_.time+=60*60*24*7*week+(int)sec; 176 | t2.t_.sec=sec-(int)sec; 177 | return t2; 178 | } 179 | 180 | double GTime::time2gpst(GTime t2, int *week, int *dow) { 181 | GTime t0= GTime::ymdhms2time(gpst0); 182 | 183 | time_t sec=t2.t_.time-t0.t_.time; 184 | int w=(int)(sec/(86400*7)); 185 | int day=(int)((sec-w*86400*7)/86400); 186 | 187 | if(week) *week=w; 188 | if(dow) *dow=day; 189 | 190 | return (double)(sec-w*86400*7)+t2.t_.sec; 191 | } 192 | 193 | GTime GTime::galt2time(int week, double sec) { 194 | GTime t2= GTime::ymdhms2time(gst0); 195 | if(sec<-1E9||sec>1E9){ 196 | sec=0; 197 | } 198 | t2.t_.time=60*60*24*7*week+(int)sec; 199 | t2.t_.sec=sec-(int)sec; 200 | return t2; 201 | } 202 | 203 | double GTime::time2galt(GTime t2, int *week, int *dow) { 204 | GTime t0= GTime::ymdhms2time(gst0); 205 | 206 | time_t sec=t2.t_.time-t0.t_.time; 207 | int w=(int)(sec/(86400*7)); 208 | int day=(int)((sec-w*86400*7)/86400); 209 | 210 | if(week) *week=w; 211 | if(dow) *dow=day; 212 | 213 | return (double)(sec-w*86400*7)+t2.t_.sec; 214 | } 215 | 216 | GTime GTime::bdt2time(int week, double sec) { 217 | GTime t2= GTime::ymdhms2time(bdt0); 218 | if(sec<-1E9||sec>1E9){ 219 | sec=0; 220 | } 221 | t2.t_.time=60*60*24*7*week+(int)sec; 222 | t2.t_.sec=sec-(int)sec; 223 | return t2; 224 | } 225 | 226 | double GTime::time2bdt(GTime t2, int *week, int *dow) { 227 | GTime t0= GTime::ymdhms2time(bdt0); 228 | 229 | time_t sec=t2.t_.time-t0.t_.time; 230 | int w=(int)(sec/(86400*7)); 231 | int day=(int)((sec-w*86400*7)/86400); 232 | 233 | if(week) *week=w; 234 | if(dow) *dow=day; 235 | 236 | return (double)(sec-w*86400*7)+t2.t_.sec; 237 | } 238 | 239 | int GTime::str2time(const char *s, int i, int n,const char *sep1, const char *sep2, GTime &t2) { 240 | double ep[6]; 241 | char str[256],*p=str; 242 | const char *s1= reinterpret_cast(' '),*s2= reinterpret_cast(' '); 243 | 244 | if(i<0||(int)strlen(s)=0;){ 247 | *p++=*s++; 248 | } 249 | *p='\0'; 250 | 251 | if(sep1) s1=sep1; 252 | if(sep2) s2=sep2; 253 | int read_count= sscanf(str,"%lf-%lf-%lf %lf:%lf:%lf", 254 | ep,ep+1,ep+2,ep+3,ep+4,ep+5); 255 | if(read_count<6) return -1; 256 | 257 | if(ep[0]<100) ep[0]+=ep[0]<80?2000:1900; 258 | 259 | t2=GTime::ymdhms2time(ep); 260 | 261 | return 1; 262 | } 263 | 264 | void GTime::time2str(GTime t2, const char *sep1, const char *sep2, int n, char *s) { 265 | double ep[6]; 266 | 267 | if(n<0) n=0; 268 | else if(n>12) n=12; 269 | 270 | if(1-t2.t_.sec<0.5/pow(10,n)){ 271 | t2.t_.time++; 272 | t2.t_.sec=0; 273 | } 274 | 275 | time2ymdhms(t2,ep); 276 | 277 | const char *s1= reinterpret_cast(' '),*s2= reinterpret_cast(' '); 278 | if(sep1) s1=sep1; 279 | if(sep2) s2=sep2; 280 | sprintf(s,"%04.0f%c%02.0f%c%02.0f %02.0f%c%02.0f%c%0*.*f", 281 | ep[0],s1,ep[1],s1,ep[2],ep[3],s2,ep[4],s2,n<=0?2:n+3,n<=0?0:n,ep[5]); 282 | } 283 | 284 | GTime GTime::yds2time(const double *yds) { 285 | int year =(int)yds[0]; 286 | int doy =(int)yds[1]; 287 | double sec=yds[2]; 288 | 289 | if(year<1970||2099=2099) return 0; 358 | 359 | if (mon>2) {i=yr; j=mon; } 360 | else {i=yr-1;j=mon+12; } 361 | 362 | double day=ep[2] 363 | +hr/24 364 | +min/24/60 365 | +sec/24/60/60; 366 | 367 | double jd = (floor(365.25*i)+floor(30.6001*(j+1))+day+1720981.5); 368 | 369 | return jd; 370 | } 371 | 372 | double GTime::gpst2mjd(GTime t2) { 373 | double ep[6]; 374 | time2ymdhms(t2,ep); 375 | double jd= ymdhms2jd(ep); 376 | return jd-JD2MJD; 377 | } 378 | 379 | double GTime::utc2mjd(GTime t2) { 380 | double ep[6]; 381 | time2ymdhms(t2,ep); 382 | double jd= ymdhms2jd(ep); 383 | return jd-JD2MJD; 384 | } -------------------------------------------------------------------------------- /src/common/GTime.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 8/27/22. 3 | // 4 | 5 | #ifndef FUSING_GTIME_H 6 | #define FUSING_GTIME_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | 14 | #include "Constants.h" 15 | 16 | extern const double gpst0[]; /* gps time reference */ 17 | extern const double gst0 []; /* galileo system time reference */ 18 | extern const double bdt0 []; /* beidou time reference */ 19 | extern const double leaps[MAXLEAPS+1][7]; 20 | 21 | typedef struct { 22 | time_t time; 23 | double sec; 24 | }timebase_t; 25 | 26 | class GTime { 27 | 28 | public: 29 | GTime() = default; 30 | ~GTime() = default; 31 | GTime(int n,double sec){ 32 | t_.time=n; 33 | t_.sec=sec; 34 | } 35 | GTime(const double *ep){ 36 | if(ep){ 37 | *this=ymdhms2time(ep); 38 | } 39 | } 40 | 41 | bool operator == (const GTime &t2) const{ 42 | if(this->t_.time!=t2.t_.time) return false; 43 | if(this->t_.time!=t2.t_.time) return false; 44 | else return true; 45 | } 46 | 47 | bool operator != (const GTime &t2) const { 48 | return !(*this==t2); 49 | } 50 | 51 | bool operator < (const GTime &t2) const { 52 | if(this->t_.timet_.time>t2.t_.time) return false; 54 | if(this->t_.sec (const GTime &t2) const{ 59 | if(this->t_.time>t2.t_.time) return true; 60 | if(this->t_.timet_.sec>t2.t_.sec) return true; 62 | else return false; 63 | } 64 | 65 | GTime operator + (const double t) const{ 66 | GTime tt=*this; 67 | tt.t_.sec+=t; 68 | 69 | double frac_sec=tt.t_.sec-(int)tt.t_.sec; 70 | double int_sec=tt.t_.sec-frac_sec; 71 | 72 | tt.t_.time+=int_sec; 73 | tt.t_.sec-=int_sec; 74 | 75 | if(tt.t_.sec<0){ 76 | tt.t_.sec+=1; 77 | tt.t_.time-=1; 78 | } 79 | 80 | return tt; 81 | } 82 | 83 | GTime operator + (const int t) const{ 84 | return *this+(double)t; 85 | } 86 | 87 | GTime& operator += (const double t) 88 | { 89 | *this=*this+t; 90 | return *this; 91 | } 92 | 93 | GTime operator - (const GTime t2) const{ 94 | GTime tt=*this; 95 | tt.t_.time -= t2.t_.time; 96 | tt.t_.sec -= t2.t_.sec; 97 | 98 | if(tt.t_.sec<0){ 99 | tt.t_.time-=1; 100 | tt.t_.sec+=1; 101 | } 102 | 103 | return tt; 104 | } 105 | 106 | GTime operator - (const double t2) const{ 107 | GTime tt=*this+(-t2); 108 | return tt; 109 | } 110 | 111 | operator double() const{ 112 | return this->t_.time + this->t_.sec; 113 | } 114 | 115 | GTime& operator ++(int){ 116 | this->t_.time++; 117 | return *this; 118 | } 119 | 120 | double getSow() const { 121 | return time2gpst(*this, nullptr, nullptr); 122 | } 123 | 124 | public: 125 | 126 | GTime gpst2utc(); 127 | GTime utc2gpst(); 128 | GTime gpst2bdt(); 129 | GTime bdt2gpst(); 130 | 131 | static GTime gpst2utc(GTime t2); 132 | static GTime utc2gpst(GTime t2); 133 | static GTime gpst2bdt(GTime t2); 134 | static GTime bdt2gpst(GTime t2); 135 | 136 | static GTime gpst2time(int week,double sec); 137 | static double time2gpst(GTime t2,int *week,int *dow); 138 | static GTime galt2time(int week,double sec); 139 | static double time2galt(GTime t2,int *week,int *dow); 140 | static GTime bdt2time(int week,double sec); 141 | static double time2bdt(GTime t2,int *week,int *dow); 142 | 143 | static GTime yrdoy2time(int year,int doy); 144 | static int time2yrdoy(GTime t2, int *year); 145 | static GTime ymdhms2time(const double *ep); 146 | static void time2ymdhms(GTime t2,double *ep); 147 | 148 | static GTime yds2time(const double *yds); 149 | static void jd2ymdhms(double jd,double *ep); 150 | static double ymdhms2jd(const double *ep); 151 | static double gpst2mjd(GTime t2); 152 | static double utc2mjd(GTime t2); 153 | 154 | static int str2time(const char *s,int i,int n,const char *sep1, const char *sep2,GTime& t2); 155 | static void time2str(GTime t2,const char *sep1,const char *sep2,int n,char *s); 156 | 157 | private: 158 | timebase_t t_={0}; 159 | }; 160 | 161 | 162 | #endif //FUSING_GTIME_H 163 | -------------------------------------------------------------------------------- /src/common/Logger.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 8/28/22. 3 | // 4 | 5 | #include "easylogging++.h" 6 | INITIALIZE_EASYLOGGINGPP 7 | #include "Logger.h" 8 | 9 | extern string logTime(double sow,int ins_epoch,int ig_epoch) 10 | { 11 | char s[124]; 12 | sprintf(s,"%9.3f-[%d %d]:",sow,ins_epoch,ig_epoch); 13 | 14 | return s; 15 | } 16 | 17 | extern void initLogger(int argc, char **argv, string log_conf_file,int debug_level) 18 | { 19 | START_EASYLOGGINGPP(argc,argv); 20 | 21 | // defaultConf.setToDefault(); 22 | 23 | if(log_conf_file.empty()){ 24 | el::Configurations defaultConf; 25 | 26 | el::Loggers::addFlag(el::LoggingFlag::DisableApplicationAbortOnFatalLog); 27 | /// 28 | el::Loggers::addFlag(el::LoggingFlag::HierarchicalLogging); 29 | /// 30 | el::Loggers::addFlag(el::LoggingFlag::ColoredTerminalOutput); 31 | 32 | el::Loggers::setLoggingLevel(static_cast(debug_level)); 33 | 34 | defaultConf.setGlobally(el::ConfigurationType::Enabled,"true"); 35 | defaultConf.setGlobally(el::ConfigurationType::ToFile,"true"); 36 | defaultConf.setGlobally(el::ConfigurationType::ToStandardOutput,"true"); 37 | defaultConf.setGlobally(el::ConfigurationType::Format,"%datetime{%s:%g} [%level] %msg"); 38 | // defaultConf.setGlobally(el::ConfigurationType::Filename,"log/%datatime{%Y-%M-%d}.log"); 39 | defaultConf.setGlobally(el::ConfigurationType::MillisecondsWidth,"3"); 40 | defaultConf.setGlobally(el::ConfigurationType::PerformanceTracking,"false"); 41 | defaultConf.setGlobally(el::ConfigurationType::MaxLogFileSize,"1048576"); 42 | defaultConf.setGlobally(el::ConfigurationType::LogFlushThreshold,"0"); 43 | 44 | /*trace*/ 45 | defaultConf.set(el::Level::Trace,el::ConfigurationType::ToStandardOutput,"false"); 46 | defaultConf.set(el::Level::Trace,el::ConfigurationType::ToFile,"true"); 47 | 48 | /*debug*/ 49 | defaultConf.set(el::Level::Debug,el::ConfigurationType::ToStandardOutput,"false"); 50 | defaultConf.set(el::Level::Debug,el::ConfigurationType::ToFile,"true"); 51 | 52 | /*fatal*/ 53 | defaultConf.set(el::Level::Fatal,el::ConfigurationType::ToStandardOutput,"false"); 54 | defaultConf.set(el::Level::Fatal,el::ConfigurationType::ToFile,"true"); 55 | 56 | /*error*/ 57 | defaultConf.set(el::Level::Trace,el::ConfigurationType::ToStandardOutput,"false"); 58 | defaultConf.set(el::Level::Trace,el::ConfigurationType::ToFile,"true"); 59 | 60 | /*warning*/ 61 | defaultConf.set(el::Level::Warning,el::ConfigurationType::ToStandardOutput,"false"); 62 | defaultConf.set(el::Level::Warning,el::ConfigurationType::ToFile,"true"); 63 | 64 | /*info*/ 65 | defaultConf.set(el::Level::Info,el::ConfigurationType::ToStandardOutput,"true"); 66 | defaultConf.set(el::Level::Info,el::ConfigurationType::ToFile,"false"); 67 | 68 | /*verbose*/ 69 | defaultConf.set(el::Level::Verbose,el::ConfigurationType::ToStandardOutput,"false"); 70 | defaultConf.set(el::Level::Verbose,el::ConfigurationType::ToFile,"true"); 71 | el::Loggers::reconfigureLogger("default", defaultConf); 72 | } 73 | else{ 74 | el::Configurations conf(log_conf_file); 75 | // el::Loggers::reconfigureLogger("default",conf); 76 | // el::Configurations c; 77 | // c.setToDefault(); 78 | // el::Loggers::reconfigureLogger("default", c); 79 | 80 | } 81 | 82 | } 83 | 84 | extern void setLogFile(vector log_paths,int log_level) 85 | { 86 | el::Configurations defaultConf; 87 | 88 | if(static_cast(log_level) == el::Level::Global){ 89 | defaultConf.setGlobally(el::ConfigurationType::Filename,log_paths[0]); 90 | } 91 | else{ 92 | /*trace*/ 93 | if(log_paths.size()==4){ 94 | defaultConf.set(el::Level::Trace,el::ConfigurationType::Filename,log_paths[0]); 95 | defaultConf.set(el::Level::Debug,el::ConfigurationType::Filename,log_paths[1]); 96 | defaultConf.set(el::Level::Fatal,el::ConfigurationType::Filename,log_paths[2]); 97 | defaultConf.set(el::Level::Error,el::ConfigurationType::Filename,log_paths[2]); 98 | defaultConf.set(el::Level::Warning,el::ConfigurationType::Filename,log_paths[2]); 99 | defaultConf.set(el::Level::Verbose,el::ConfigurationType::Filename,log_paths[3]); 100 | } 101 | else if(log_paths.size()==3){ 102 | defaultConf.set(el::Level::Trace,el::ConfigurationType::Filename,log_paths[0]); 103 | defaultConf.set(el::Level::Debug,el::ConfigurationType::Filename,log_paths[1]); 104 | defaultConf.set(el::Level::Fatal,el::ConfigurationType::Filename,log_paths[2]); 105 | defaultConf.set(el::Level::Error,el::ConfigurationType::Filename,log_paths[2]); 106 | defaultConf.set(el::Level::Warning,el::ConfigurationType::Filename,log_paths[2]); 107 | } 108 | else if(log_paths.size()==2){ 109 | defaultConf.set(el::Level::Trace,el::ConfigurationType::Filename,log_paths[0]); 110 | defaultConf.set(el::Level::Debug,el::ConfigurationType::Filename,log_paths[1]); 111 | } 112 | else if(log_paths.size()==1){ 113 | defaultConf.set(el::Level::Trace,el::ConfigurationType::Filename,log_paths[0]); 114 | } 115 | } 116 | el::Loggers::reconfigureLogger("default", defaultConf); 117 | } -------------------------------------------------------------------------------- /src/common/Logger.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 8/28/22. 3 | // 4 | 5 | #ifndef FUSING_LOGGER_H 6 | #define FUSING_LOGGER_H 7 | 8 | #include 9 | #include 10 | 11 | #include "easylogging++.h" 12 | 13 | using namespace std; 14 | 15 | extern string logTime(double sow,int ins_epoch,int ig_epoch); 16 | extern void initLogger(int argc, char **argv, string log_conf_file,int debug_level); 17 | extern void setLogFile(vector log_paths,int log_level); 18 | 19 | #endif //FUSING_LOGGER_H 20 | -------------------------------------------------------------------------------- /src/common/ProcessBar.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 12/9/22. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include "ProcessBar.h" 9 | 10 | 11 | ProcessBar::ProcessBar(/* args */) {} 12 | 13 | ProcessBar::ProcessBar(long totalNum) 14 | { 15 | totalNum_ = totalNum; 16 | } 17 | 18 | ProcessBar::~ProcessBar() {} 19 | 20 | void ProcessBar::Init(long totalNum) 21 | { 22 | totalNum_ = totalNum; 23 | } 24 | 25 | void ProcessBar::Progress(long num) 26 | { 27 | int currentIndex = num * 100 / step_ / totalNum_; 28 | for (int j = 0; j < iconMaxNum_; ++j) 29 | { 30 | if (j < currentIndex) { 31 | printf("\033[1;42m%c\033[0m", icon_); 32 | } else if (j == currentIndex) { 33 | printf("\033[1;44m%c\033[0m", icon_); 34 | } else { 35 | printf("%c", ' '); 36 | } 37 | } 38 | 39 | printf("\033[32m %.1f%%\033[0m", 100.0 * num / totalNum_); 40 | fflush(stdout); 41 | 42 | usleep(interval_); 43 | if (num != totalNum_) { 44 | for (int j = 0; j < (iconMaxNum_ + 3000); ++j) 45 | { 46 | printf("\b"); 47 | } 48 | fflush(stdout); 49 | } else { 50 | printf("\n"); 51 | } 52 | } -------------------------------------------------------------------------------- /src/common/ProcessBar.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 12/9/22. 3 | // 4 | 5 | #ifndef FUSING_PROCESSBAR_H 6 | #define FUSING_PROCESSBAR_H 7 | 8 | 9 | class ProcessBar { 10 | private: 11 | char icon_{' '}; 12 | long totalNum_; 13 | int step_ = 5; 14 | int iconMaxNum_ = 45; 15 | int interval_ = 10; 16 | public: 17 | ProcessBar(/* args */); 18 | ProcessBar(long totalNum); 19 | void Init(long totalNum); 20 | void Progress(long currentNum); 21 | ~ProcessBar(); 22 | }; 23 | 24 | 25 | #endif //FUSING_PROCESSBAR_H 26 | -------------------------------------------------------------------------------- /src/common/Rotation.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 8/27/22. 3 | // 4 | 5 | #include "Rotation.h" 6 | 7 | Quaterniond Rotation::rv2quat(Vector3d rv){ 8 | double angle = rv.norm(); 9 | Vector3d vec = rv.normalized(); 10 | return Quaterniond(Eigen::AngleAxisd(angle, vec)); 11 | } 12 | 13 | Vector3d Rotation::quat2rv(Eigen::Quaterniond quat) { 14 | Eigen::AngleAxisd axisd(quat); 15 | return axisd.angle() * axisd.axis(); 16 | } 17 | 18 | Matrix3d Rotation::quat2dcm(Eigen::Quaterniond quat) { 19 | return quat.toRotationMatrix(); 20 | } 21 | 22 | Quaterniond Rotation::dcm2quat(Eigen::Matrix3d dcm) { 23 | return Quaterniond(dcm); 24 | } 25 | 26 | Matrix3d Rotation::att2Cbn(Eigen::Vector3d att, E_AttDefination att_type) { 27 | Matrix3d Cbn; 28 | switch (att_type) { 29 | case E_AttDefination::NED_FRD: /*321: roll pitch yaw*/ 30 | Cbn=Matrix3d(Eigen::AngleAxisd(att[2], Vector3d::UnitZ()) * 31 | Eigen::AngleAxisd(att[1], Vector3d::UnitY()) * 32 | Eigen::AngleAxisd(att[0], Vector3d::UnitX())); 33 | break; 34 | case E_AttDefination::ENU_RFU: /*312: pitch roll yaw*/ 35 | Cbn=Matrix3d(Eigen::AngleAxisd(att[2], Vector3d::UnitZ()) * 36 | Eigen::AngleAxisd(att[0], Vector3d::UnitX()) * 37 | Eigen::AngleAxisd(att[1], Vector3d::UnitY())); 38 | } 39 | return Cbn; 40 | } 41 | 42 | Vector3d Rotation::Cbn2att(Eigen::Matrix3d Cbn, E_AttDefination att_type) { 43 | Vector3d att; 44 | 45 | switch (att_type) { 46 | case E_AttDefination::NED_FRD: 47 | att[1]=atan(-Cbn(2,0)/sqrt(Cbn(2,1)*Cbn(2,1)+Cbn(2,2)*Cbn(2,2))); /*roll*/ 48 | 49 | if (Cbn(2,0)<=-0.999){ 50 | att[0]=atan2(Cbn(2,1),Cbn(2,2)); /*pitch*/ 51 | att[2]=atan2((Cbn(1,2)-Cbn(0,1)),(Cbn(0,2)+Cbn(1,1))); /*yaw*/ 52 | } else if (Cbn(2,0)>=0.999){ 53 | att[0]=atan2(Cbn(2,1),Cbn(2,2)); 54 | att[2]=M_PI+atan2((Cbn(1,2)+Cbn(0, 1)),(Cbn(0,2)-Cbn(1,1))); 55 | } else { 56 | att[0]=atan2(Cbn(2,1),Cbn(2,2)); 57 | att[2]=atan2(Cbn(1,0),Cbn(0,0)); 58 | } 59 | break; 60 | 61 | case E_AttDefination::ENU_RFU: 62 | att[0] = asin(Cbn(2,1)); /*pitch*/ 63 | if(fabs(Cbn(2,1))<=0.999){ 64 | att[1]=-atan2(Cbn(2,0),Cbn(2,2)); /*roll*/ 65 | att[2]=-atan2(Cbn(0,1),Cbn(1,1)); /*yaw*/ 66 | } 67 | else{ 68 | att[1]= atan2(Cbn(0,2),Cbn(0,0)); /*pitch*/ 69 | att[2]=0.0; /*yaw*/ 70 | } 71 | break; 72 | } 73 | 74 | // heading 0~2PI 75 | if (att[2] < 0) { 76 | att[2] = M_PI * 2 + att[2]; 77 | } 78 | 79 | return att; 80 | } 81 | 82 | Matrix3d Rotation::att2Cbe(Eigen::Vector3d att, Eigen::Vector3d llh, E_AttDefination att_type) { 83 | Matrix3d Cne,Cbn; 84 | Cne = getCne(llh,att_type); 85 | Cbn = att2Cbn(att,att_type); 86 | return Cne*Cbn; 87 | } 88 | 89 | Vector3d Rotation::Cbe2att(Eigen::Matrix3d Cbe, Eigen::Vector3d llh, E_AttDefination att_type) { 90 | Matrix3d Cne,Cbn; 91 | Cne = getCne(llh,att_type); 92 | Cbn = Cne.transpose()*Cbe; 93 | return Cbn2att(Cbn,att_type); 94 | } 95 | 96 | Quaterniond Rotation::att2qbn(Eigen::Vector3d att, E_AttDefination att_type) { 97 | Quaterniond qbn; 98 | switch (att_type) { 99 | case E_AttDefination::NED_FRD: 100 | qbn=Quaterniond(Eigen::AngleAxisd(att[2], Vector3d::UnitZ()) * 101 | Eigen::AngleAxisd(att[1], Vector3d::UnitY()) * 102 | Eigen::AngleAxisd(att[0], Vector3d::UnitX())); 103 | break; 104 | case E_AttDefination::ENU_RFU: 105 | qbn=Quaterniond(Eigen::AngleAxisd(att[2], Vector3d::UnitZ()) * 106 | Eigen::AngleAxisd(att[0], Vector3d::UnitX()) * 107 | Eigen::AngleAxisd(att[1], Vector3d::UnitY())); 108 | break; 109 | 110 | } 111 | return qbn; 112 | } 113 | 114 | Vector3d Rotation::qbn2att(Eigen::Quaterniond quat, E_AttDefination att_type) { 115 | return Cbn2att(quat.toRotationMatrix(),att_type); 116 | } 117 | 118 | Quaterniond Rotation::att2qbe(Eigen::Vector3d att, Eigen::Vector3d llh, E_AttDefination att_type) { 119 | Matrix3d Cbe= att2Cbe(llh,att,att_type); 120 | return dcm2quat(Cbe); 121 | } 122 | 123 | Vector3d Rotation::qbe2att(Eigen::Quaterniond quat, Eigen::Vector3d llh, E_AttDefination att_type) { 124 | Matrix3d Cbe= quat2dcm(quat); 125 | return Cbe2att(Cbe,llh,att_type); 126 | } 127 | 128 | Matrix3d Rotation::getCne(Eigen::Vector3d llh, E_AttDefination att_type) { 129 | Matrix3d Cne=Matrix3d::Zero(); 130 | double sinp=sin(llh[0]),cosp=cos(llh[0]); 131 | double sinl=sin(llh[1]),cosl=cos(llh[1]); 132 | switch (att_type) { 133 | case E_AttDefination::NED_FRD: 134 | Cne(0,0)=-sinp*cosl; Cne(0,1)=-sinl; Cne(0,2)=-cosp*cosl; 135 | Cne(1,0)=-sinp*sinl; Cne(1,1)=cosl; Cne(1,2)=-cosp*sinl; 136 | Cne(2,0)=cosp; Cne(2,1)=0.0; Cne(2,2)=-sinp; 137 | break; 138 | case E_AttDefination::ENU_RFU: 139 | Cne(0,0)=-sinl; Cne(0,1)=-sinp*cosl; Cne(0,2)=cosp*cosl; 140 | Cne(1,0)=cosl; Cne(1,1)=-sinp*sinl; Cne(1,2)=cosp*sinl; 141 | Cne(2,0)=0.0; Cne(2,1)=cosp; Cne(2,2)=sinp; 142 | break; 143 | default: 144 | return Cne; 145 | } 146 | return Cne; 147 | } 148 | 149 | Matrix3d Rotation::skewSymmetric(Eigen::Vector3d vec) { 150 | Matrix3d mat; 151 | mat << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0; 152 | return mat; 153 | } -------------------------------------------------------------------------------- /src/common/Rotation.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 8/27/22. 3 | // 4 | 5 | #ifndef FUSING_ROTATION_H 6 | #define FUSING_ROTATION_H 7 | 8 | #include "Enums.h" 9 | #include "EigenInc.h" 10 | 11 | class Rotation { 12 | public: 13 | Rotation() = default; 14 | ~Rotation() = default; 15 | 16 | public: 17 | static Quaterniond rv2quat(Vector3d rv); 18 | static Vector3d quat2rv(Quaterniond quat); 19 | static Matrix3d quat2dcm(Quaterniond quat); 20 | static Quaterniond dcm2quat(Matrix3d dcm); 21 | static Matrix3d att2Cbn(Vector3d att, E_AttDefination att_type); 22 | static Vector3d Cbn2att(Matrix3d Cbn, E_AttDefination att_type); 23 | static Matrix3d att2Cbe(Vector3d att,Vector3d llh, E_AttDefination att_type); 24 | static Vector3d Cbe2att(Matrix3d Cbe, Vector3d llh, E_AttDefination att_type); 25 | static Quaterniond att2qbn(Vector3d att,E_AttDefination att_type); 26 | static Vector3d qbn2att(Quaterniond quat,E_AttDefination att_type); 27 | static Quaterniond att2qbe(Vector3d att,Vector3d llh,E_AttDefination att_type); 28 | static Vector3d qbe2att(Quaterniond quat,Vector3d llh, E_AttDefination att_type); 29 | 30 | static Matrix3d getCne(Vector3d llh,E_AttDefination att_type); 31 | static Matrix3d skewSymmetric(Vector3d vec); 32 | }; 33 | 34 | 35 | #endif //FUSING_ROTATION_H 36 | -------------------------------------------------------------------------------- /src/common/Statistics.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 12/10/22. 3 | // 4 | 5 | #include "Logger.h" 6 | #include "Statistics.h" 7 | 8 | using namespace std; 9 | 10 | extern double calcSum(const double *data,int n,bool abs) 11 | { 12 | double sum=0.0; 13 | if(abs){ 14 | for(int i=0;imax) max=fabs(data[i]); 28 | } 29 | return max; 30 | } 31 | 32 | extern double calcAve(const double *data,int n,bool abs) 33 | { 34 | double sum= calcSum(data,n,abs); 35 | return sum/n; 36 | } 37 | 38 | 39 | extern double calcRMSE(const double *data,int n,bool abs) 40 | { 41 | double sum= calcSum(data,n,abs); 42 | return sqrt(fabs(sum)/n); 43 | } 44 | 45 | extern double calcSTD(const double *data,int n,bool abs) 46 | { 47 | double ave= calcAve(data,n,abs); 48 | 49 | double sum=0.0; 50 | for(int i=0;ifabs(*q2); 61 | } 62 | 63 | extern double calcCEP(const double *data,int n,int nSigma) 64 | { 65 | double *new_data; 66 | 67 | if(nSigma<1||nSigma>3) return 0.0; 68 | 69 | if (!(new_data=(double *)malloc(sizeof(double)*n))) { 70 | LOG(ERROR)<<"matrix memory allocation error\n"; 71 | } 72 | 73 | memcpy(new_data,data,sizeof(double)*n); 74 | 75 | for(int i=0;i 9 | #include 10 | #include 11 | 12 | extern double calcSum(const double *data,int n,bool abs); 13 | extern double calcMax(const double *data,int n); 14 | extern double calcAve(const double *data,int n,bool abs); 15 | extern double calcRMSE(const double *data,int n,bool abs); 16 | extern double calcSTD(const double *data,int n,bool abs); 17 | extern double calcCEP(const double *data,int n,int nSigma); 18 | 19 | #endif //FUSING_STATISTICS_H 20 | -------------------------------------------------------------------------------- /src/fusing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.0) 2 | 3 | project(${FusingLib}) 4 | 5 | file(GLOB_RECURSE header_files ${libFusingSrc}/*.h) 6 | file(GLOB_RECURSE source_files ${libFusingSrc}/*.cc) 7 | 8 | include_directories(${libFusingSrc} 9 | ${libFImuSrc} 10 | ${libFGnssSrc} 11 | ${libMiscSrc}) 12 | 13 | add_library(${PROJECT_NAME} ${header_files} ${source_files}) 14 | 15 | target_link_libraries(${PROJECT_NAME} ${CommonLib} 16 | ${FImuLib} 17 | ${FGnssLib} 18 | ${MiscLib}) 19 | 20 | if (CMAKE_SYSTEM_NAME MATCHES "Windows") 21 | link_directories(${ROOT}/build/Lib/Debug) 22 | link_directories(${ROOT}/build/Lib/Release) 23 | link_directories(${ROOT}/build/Lib/RelWithDebInfo) 24 | link_directories(${ROOT}/build/Lib/MinSizeRel) 25 | else () 26 | link_directories(${ROOT}/build/Lib) 27 | endif () 28 | -------------------------------------------------------------------------------- /src/fusing/Fbs.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 10/16/22. 3 | // 4 | 5 | #include "OutSol.h" 6 | #include "Rotation.h" 7 | #include "Coordinate.h" 8 | #include "Fusing.h" 9 | #include "Fbs.h" 10 | #include "jprogress_bar.hpp" 11 | 12 | static int cmpState(const void *p1,const void *p2){ 13 | ins_t *q1=(ins_t *)p1,*q2=(ins_t *)p2; 14 | double tt=q1->state.sow-q2->state.sow; 15 | return tt<0?-1:1; 16 | } 17 | 18 | extern int sortTrack(track_t *track) 19 | { 20 | if(track->n<=0) return 0; 21 | qsort(track->data,track->n,sizeof(ins_t), cmpState); 22 | return 1; 23 | } 24 | 25 | static bool matchBwdSol(double sow_solf,int *idx,const track_t *solb) 26 | { 27 | bool stat=false; 28 | int i; 29 | double dt,sow_solb; 30 | 31 | dt=fabs(solb->data[2].state.sow-solb->data[1].state.sow); 32 | 33 | for(i=*idx>5?*idx-3:*idx;in-1;i++){ 34 | sow_solb=solb->data[i].state.sow; 35 | if(fabs(sow_solf-sow_solb)1.0){ 41 | break; 42 | } 43 | } 44 | 45 | return stat; 46 | } 47 | 48 | static Vector3d attSmoother(const Vector3d &pos,const Quaterniond &qf, const Matrix3d &Qf, const Quaterniond &qb, 49 | const Matrix3d &Qb, Vector3d &xs, Matrix3d& Qs,E_InsNavCoord nav_coord,E_AttDefination att_def) 50 | { 51 | Quaterniond qs; 52 | Vector3d phi; 53 | phi=Rotation::quat2rv(qf*qb.conjugate()); 54 | Matrix3d Q; 55 | Qs=Qf+Qb; 56 | Q=Qf*Qs.inverse(); 57 | qs=Rotation::rv2quat(-Q*phi)*qf; 58 | 59 | if(nav_coord==+E_InsNavCoord::LLH){ 60 | xs=Rotation::qbn2att(qs,att_def); 61 | } 62 | else if(nav_coord==+E_InsNavCoord::ECEF){ 63 | Vector3d llh; 64 | llh=Coordinate::ecef2llh(pos); 65 | xs=Rotation::qbe2att(qs,llh,att_def); 66 | } 67 | Qs=Q*Qb; 68 | 69 | return phi; 70 | } 71 | 72 | extern bool fbSmoother(FILE *fp,fusing_t& fusing) 73 | { 74 | int i,idx=0; 75 | 76 | if(fusing.solb.n==0||fusing.solf.n==0) return false; 77 | sortTrack(&fusing.solb); 78 | 79 | LOG(DEBUG)<<"start forward-backward solution fusion: solf.n = "< 9 | 10 | struct fusing_t; 11 | struct track_t; 12 | 13 | extern int sortTrack(track_t *track); 14 | extern bool fbSmoother(FILE *fp,fusing_t& fusing); 15 | 16 | #endif //FUSING_FBSMOOTH_H 17 | -------------------------------------------------------------------------------- /src/fusing/Fusing.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 10/2/22. 3 | // 4 | 5 | #ifndef FUSING_FUSING_H 6 | #define FUSING_FUSING_H 7 | 8 | #include "Gnss.h" 9 | #include "Imu.h" 10 | #include "Ins.h" 11 | #include "rtklib.h" 12 | 13 | #define NO_ZERO_FLAG 1E-32 14 | 15 | struct track_t{ 16 | int n,nmax; 17 | ins_t *data; 18 | }; 19 | 20 | struct fusingMeas_t{ 21 | imu_t imu; 22 | pv_t pvs; 23 | obs_t obs; 24 | nav_t nav; 25 | }; 26 | 27 | typedef struct{ 28 | int imu; 29 | int pv; 30 | int rover; 31 | int base; 32 | }measIdx_t; 33 | 34 | struct epMeas_t{ 35 | epImuData_t imu; 36 | pvData_t pv; 37 | vector gnss; 38 | measIdx_t idx; 39 | }; 40 | 41 | typedef struct{ 42 | int nx,nix; 43 | int na; 44 | VectorXd x, xa; 45 | MatrixXd P, Pa; 46 | MatrixXd F, Q, G; 47 | }kf_t; 48 | 49 | struct fusing_t{ 50 | fusingopt_t opts; 51 | rtk_t rtk; 52 | ins_t ins,ig_ins; 53 | nav_t nav; 54 | 55 | int week; 56 | epMeas_t ep_sensors_meas; 57 | kf_t ep_kf; 58 | sol_t ep_sol; 59 | track_t solf={0},solb={0}; 60 | 61 | bool iglc_sol=false,iglc_obs=false,igstc=false,igtc=false; 62 | bool isSPP=false,isDGPS=false,isPPK=false,isPPP=false,isPPP_AR=false; 63 | }; 64 | 65 | extern int xnP(); 66 | extern int xnV(); 67 | extern int xnA(); 68 | extern int xnBg(); 69 | extern int xnBa(); 70 | extern int xnSg(bool est); 71 | extern int xnSa(bool est); 72 | extern int xnClk(E_GnssMode mode,bool sd_gnss); 73 | extern int xnClkDrift(E_GnssMode mode,bool sd_gnss,bool dop_aid); 74 | extern int xnTrp(E_GnssMode mode,E_TropOpt opt); 75 | extern int xnIon(E_GnssMode mode,E_IonoOpt opt); 76 | extern int xnIfcb(E_GnssMode mode,int nf); 77 | extern int xnAmb(const fusingopt_t &opt); 78 | extern int xnX(const fusingopt_t &opt); 79 | extern int xnFrq(const fusingopt_t &opt); 80 | extern int xnIns(const fusingopt_t &opt); 81 | 82 | extern int xiP(); 83 | extern int xiV(); 84 | extern int xiA(); 85 | extern int xiBg(); 86 | extern int xiBa(); 87 | extern int xiSg(); 88 | extern int xiSa(const imuopt_t &opt); 89 | extern int xiClk(const fusingopt_t &opts); 90 | extern int xiClkDrift(const fusingopt_t &opts); 91 | extern int xiTrp(const fusingopt_t &opts); 92 | extern int xiIon(const fusingopt_t &opts,int sat); 93 | extern int xiIfcb(const fusingopt_t &opts); 94 | extern int xiAmb(const fusingopt_t &opts,int sat, int f); 95 | 96 | 97 | extern bool addSolData(const state_t *sol,track_t *ref); 98 | extern void removeIGArmLever(const ins_t &ins, const Eigen::Vector3d& arm, Eigen::Vector3d &pred_pos, 99 | Eigen::Vector3d* pred_vel, bool ins2gnss,E_InsNavCoord mech_coord); 100 | extern bool loadMeasurement(fusing_t &fusing, const fusingopt_t &opts,fusingMeas_t &meas); 101 | extern bool sensorsFusion(fusing_t &fusing); 102 | extern bool sensorFusingProcess(FILE *fp_out,const fusingMeas_t &meas,fusing_t& fusing,bool back); 103 | extern bool multiSensorFusing(fusingMeas_t &meas,fusing_t& fusing); 104 | extern void initFusing(fusing_t &fusing, Config &config); 105 | extern bool insFeedback(const imuopt_t &opt,E_InsNavCoord nav_coord,E_AttDefination att_def,ins_t &ins,double *x,int nx); 106 | #endif //FUSING_FUSING_H 107 | -------------------------------------------------------------------------------- /src/fusing/LooseCouple.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 10/30/22. 3 | // 4 | 5 | #include 6 | #include "LooseCouple.h" 7 | 8 | #define SQR(x) ((x)*(x)) 9 | 10 | static default_random_engine e(time(0)); 11 | static normal_distribution n(0,0.5); 12 | 13 | static bool getHRV(const fusingopt_t &opts,const pvData_t &pv, const ins_t &ins,int nx, Eigen::MatrixXd& H, Eigen::Vector3d& v, Eigen::Matrix3d& R) { 14 | 15 | 16 | H=MatrixXd::Zero(3,nx); 17 | H.block(0,xiP(),3,3)=Matrix3d::Identity(); 18 | 19 | Vector3d pred_pos; 20 | removeIGArmLever(ins, ins.ig_lever, pred_pos, nullptr,true,opts.imu.nav_coord); 21 | 22 | R=pv.cov_pos.asDiagonal(); 23 | if(opts.imu.gnsspv_fmt==+E_GnssPvFmt::PSINS){ 24 | Vector3d pos_err; 25 | if(opts.imu.nav_coord==+E_InsNavCoord::ECEF){ 26 | pos_err=Vector3d(0.1,0.1,0.3); 27 | } 28 | else if(opts.imu.nav_coord==+E_InsNavCoord::LLH){ 29 | pos_err=Vector3d(1,1,3); 30 | } 31 | Vector3d tmp,rn=Coordinate::ecef2llh(ins.state.pos); 32 | Matrix3d Cne=Rotation::getCne(rn,E_AttDefination::ENU_RFU); 33 | tmp=Cne*pos_err; 34 | Vector3d rand_pos_err; 35 | if(opts.imu.nav_coord==+E_InsNavCoord::ECEF){ 36 | rand_pos_err=Vector3d(pos_err(0)*n(e),pos_err(1)*n(e),pos_err(2)*n(e)); 37 | } 38 | else if(opts.imu.nav_coord==+E_InsNavCoord::LLH){ 39 | rand_pos_err=Vector3d(pos_err(0)*n(e)/WGS84_RE,pos_err(1)*n(e)/WGS84_RE,pos_err(2)*n(e)); 40 | } 41 | v=ins.state.pos-pv.pos+rand_pos_err; 42 | } 43 | else{ 44 | v=pred_pos-pv.pos; 45 | R=R*10.0; 46 | } 47 | 48 | return true; 49 | } 50 | 51 | static Matrix3d igg3(const fusing_t &fusing,const MatrixXd H,const MatrixXd P,const Matrix3d R,Vector3d v) 52 | { 53 | Matrix3d new_Pyy,Pyy; 54 | Matrix3d new_R=R; 55 | double norm_v=1.5,k0=1.0,k1=2.5; 56 | 57 | Pyy=H*P*H.transpose()+R; 58 | for(int i=0;i<3;i++){ 59 | // norm_v=fabs(v(i)/sqrt(Pyy(i,i))); 60 | // if(fabs(v[i])>0.10){ 61 | // new_R*=1000000.0; 62 | // break; 63 | // } 64 | // if(norm_v>k1){ 65 | // new_R(i,i)*=1000000.0; 66 | // LOG(TRACE)<0;i--){ 15 | double gpst=pvs.data[i].sow; 16 | if(gpst>=cur_imu&&gpst<=pre_imu){ 17 | sync=true; 18 | *pv_idx=i; 19 | LOG(DEBUG)<5?*pv_idx-5:0;i1.0){ 39 | sync=false; 40 | break; 41 | } 42 | } 43 | } 44 | return sync; 45 | } 46 | 47 | extern bool igSyncObs(double pre_imu,double cur_imu,const obs_t &obs,measIdx_t *idx,bool back) 48 | { 49 | bool sync=false; 50 | int i=0; 51 | double dt1,dt2; 52 | double gpst; 53 | 54 | if(!back){ 55 | for(i=idx->rover>100?idx->rover-100:0;i=pre_imu&&gpstrover=i; 61 | LOG(TRACE)<1.0){ 65 | sync=false; 66 | break; 67 | } 68 | } 69 | } 70 | else{ 71 | for(i=idx->rover>(idx->rover-100)?idx->rover:idx->rover-100;i>=0;i--){ 72 | gpst= time2gpst(obs.data[i].time, nullptr); 73 | if(gpst>cur_imu&&gpst<=pre_imu){ 74 | sync=true; 75 | idx->rover=i; 76 | LOG(TRACE)<& gnss,bool back) 89 | { 90 | int i,nu,nr,k=0; 91 | 92 | gnss.clear(); 93 | if(idx.rover<0||idx.rover>obs.n) return 0; 94 | 95 | if(!back){ 96 | if((nu=nextobsf(&obs,&idx.rover,1))<=0) return 0; 97 | if(fusing.opts.rtklib.prc_opt.intpref) { 98 | for(;(nr=nextobsf(&obs,&idx.base, 2))>0;idx.base+=nr) 99 | if(timediff(obs.data[idx.base].time,obs.data[idx.rover].time)>-DTTOL) break; 100 | }else{ 101 | for(i=idx.base;(nr=nextobsf(&obs,&i,2)) > 0;idx.base=i,i+=nr) 102 | if(timediff(obs.data[i].time,obs.data[idx.rover].time)>DTTOL) break; 103 | } 104 | nr=nextobsf(&obs,&idx.base,2); 105 | if(nr<=0){ 106 | nr=nextobsf(&obs,&idx.base,2); 107 | } 108 | for(i=0;i0;idx.base-=nr) 122 | if (timediff(obs.data[idx.base].time,obs.data[idx.rover].time)0;idx.base=i,i-=nr) 125 | if (timediff(obs.data[i].time,obs.data[idx.rover].time)<-DTTOL) break; 126 | } 127 | nr=nextobsb(&obs,&idx.base,2); 128 | for(i=0;i& gnss,bool back); 20 | 21 | #endif //FUSING_SYNCSENSORS_H 22 | -------------------------------------------------------------------------------- /src/fusing/TightCouple.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 10/30/22. 3 | // 4 | 5 | #ifndef FUSING_TIGHTCOUPLE_H 6 | #define FUSING_TIGHTCOUPLE_H 7 | 8 | #include "Logger.h" 9 | #include "EigenInc.h" 10 | #include "Coordinate.h" 11 | #include "Rotation.h" 12 | #include "Enums.h" 13 | #include "Ins.h" 14 | #include "Gnss.h" 15 | #include "Fusing.h" 16 | 17 | extern bool igSensorTc1(fusing_t &fusing); 18 | extern bool igSensorTC(fusing_t &fusing); 19 | #endif //FUSING_TIGHTCOUPLE_H 20 | -------------------------------------------------------------------------------- /src/fusing/TimeUpdate.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 10/30/22. 3 | // 4 | 5 | #ifndef FUSING_TIMEUPDATE_H 6 | #define FUSING_TIMEUPDATE_H 7 | 8 | #include "Logger.h" 9 | #include "EigenInc.h" 10 | #include "Coordinate.h" 11 | #include "Rotation.h" 12 | #include "Enums.h" 13 | #include "Ins.h" 14 | #include "Gnss.h" 15 | #include "Fusing.h" 16 | 17 | extern void timeUpdate(const imuProperty_t &imup,fusing_t &fusing,bool back); 18 | 19 | #endif //FUSING_TIMEUPDATE_H 20 | -------------------------------------------------------------------------------- /src/misc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.0) 2 | 3 | project(${MiscLib}) 4 | 5 | file(GLOB_RECURSE header_files ${libMiscSrc}/*.h) 6 | file(GLOB_RECURSE source_files ${libMiscSrc}/*.cc) 7 | 8 | include_directories(${libMiscSrc} 9 | ${libCommonSrc} 10 | ${libFusingSrc} 11 | ${libFImuSrc} 12 | ${libFGnssSrc} 13 | ${libFusingSrc}) 14 | 15 | add_library(${PROJECT_NAME} ${header_files} ${source_files}) 16 | 17 | target_link_libraries(${PROJECT_NAME} ${CommonLib} 18 | ${FImuLib} 19 | ${FGnssLib} 20 | ${FusingLib}) 21 | 22 | if (CMAKE_SYSTEM_NAME MATCHES "Windows") 23 | link_directories(${ROOT}/build/Lib/Debug) 24 | link_directories(${ROOT}/build/Lib/Release) 25 | link_directories(${ROOT}/build/Lib/RelWithDebInfo) 26 | link_directories(${ROOT}/build/Lib/MinSizeRel) 27 | else () 28 | link_directories(${ROOT}/build/Lib) 29 | endif () 30 | -------------------------------------------------------------------------------- /src/misc/MatchFiles.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 11/19/22. 3 | // 4 | 5 | #ifndef FUSING_MATCHFILES_H 6 | #define FUSING_MATCHFILES_H 7 | 8 | #include "Enums.h" 9 | #include "Logger.h" 10 | #include "Config.h" 11 | #include "filesystem.hpp" 12 | 13 | namespace fs=ghc::filesystem; 14 | 15 | struct fusing_t; 16 | 17 | extern bool isExist(const fs::path& path, bool is_directory); 18 | extern bool matchOutFiles(const fusing_t& fusing, fusingopt_t& opts,string& sol_path,string* err_path,vector& log_path,int log_level); 19 | extern bool matchRefFiles(const fusingopt_t& opts,string site_name,string &ref_path); 20 | extern bool autoMatchFiles(const fusing_t &fusing,fusingopt_t& opts,int log_level); 21 | extern bool checkInputFiles(const fusing_t &fusing,fusingopt_t& opts,int log_level); 22 | #endif //FUSING_MATCHFILES_H 23 | -------------------------------------------------------------------------------- /src/misc/OutSol.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 10/16/22. 3 | // 4 | 5 | #include "Rotation.h" 6 | #include "Coordinate.h" 7 | #include "OutSol.h" 8 | 9 | #define SQR(x) ((x)<0.0?-(x)*(x):(x)*(x)) 10 | #define SQRT(x) ((x)<0.0||(x)!=(x)?0.0:sqrt(x)) 11 | 12 | extern void writeSol1(FILE *fp,const sol_t& sol) 13 | { 14 | double sow; 15 | sow= time2gpst(sol.time, nullptr); 16 | 17 | fprintf(fp,"%4d %8.4f %14.10f %14.10f %14.10f %8.4f %8.4f %8.4f %15.9f %15.9f %15.9f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f\n", 18 | 2140, 19 | sow, 20 | sol.rr[0]*R2D,sol.rr[1]*R2D,sol.rr[2], 21 | sol.rr[3],sol.rr[4],sol.rr[5], 22 | sol.rpy[0]*R2D,sol.rpy[1]*R2D,sol.rpy[2]*R2D, 23 | sol.bg[0]*RPS2DPH,sol.bg[1]*RPS2DPH,sol.bg[2]*RPS2DPH, 24 | sol.ba[0]*MPS22MG,sol.ba[1]*MPS22MG,sol.ba[2]*MPS22MG); 25 | } 26 | 27 | extern void writeSol(FILE *fp,const state_t& state,E_InsNavCoord mech_coord,E_AttDefination att_type,bool tc,const sol_t &sol) 28 | { 29 | Vector3d rn,vn,rpy,sow; 30 | 31 | if(mech_coord==+E_InsNavCoord::ECEF){ 32 | rn=Coordinate::ecef2llh(state.pos); 33 | Matrix3d Cne=Rotation::getCne(rn,att_type); 34 | vn=Cne.transpose()*state.vel; 35 | if(att_type==+E_AttDefination::ENU_RFU){ 36 | rpy[0]=state.att[1],rpy[1]=state.att[0],rpy[2]=2*PI-state.att[2]; 37 | double tmp=vn[0]; 38 | vn[0]=vn[1];vn[1]=tmp;vn[2]=-vn[2]; 39 | } 40 | else{ 41 | rpy=state.att; 42 | } 43 | } 44 | else if(mech_coord==+E_InsNavCoord::LLH){ 45 | rn=state.pos,vn=state.vel; 46 | Vector3d xyz; 47 | xyz=Coordinate::llh2ecef(state.pos); 48 | if(att_type==+E_AttDefination::ENU_RFU){ 49 | rpy[0]=state.att[1],rpy[1]=state.att[0],rpy[2]=2*PI-state.att[2]; 50 | vn[0]=state.vel[1],vn[1]=state.vel[0],vn[2]=-state.vel[2]; 51 | } 52 | else{ 53 | rpy=state.att; 54 | } 55 | } 56 | 57 | fprintf(fp,"%4d %8.4f %12.9f %12.9f %9.5f %8.4f %8.4f %8.4f %15.9f %15.9f %15.9f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f\n", 58 | 2140, 59 | state.sow, 60 | rn[0]*R2D,rn[1]*R2D,rn[2], 61 | vn[0],vn[1],vn[2], 62 | rpy[0]*R2D,rpy[1]*R2D,rpy[2]*R2D, 63 | state.bg[0]*RPS2DPH,state.bg[1]*RPS2DPH,state.bg[2]*RPS2DPH, 64 | state.ba[0]*MPS22MG,state.ba[1]*MPS22MG,state.ba[2]*MPS22MG); 65 | } 66 | 67 | extern void state2sol(const imuopt_t opt,const fusing_t &fusing,const ins_t &ins,sol_t &sol,int week,E_PrcMode prc_mode) 68 | { 69 | Vector3d re,ve,rpy,rn,vn; 70 | Matrix3d cov_re,cov_ve,cov_rpy; 71 | sol_t sol0={{0}}; 72 | sol=sol0; 73 | 74 | if(prc_mode!=+E_PrcMode::INS) sol=fusing.rtk.sol; 75 | if(opt.nav_coord==+E_InsNavCoord::ECEF){ 76 | re=ins.state.pos; 77 | rn=Coordinate::ecef2llh(ins.state.pos); 78 | Matrix3d Cne=Rotation::getCne(rn,opt.att_def); 79 | vn=Cne.transpose()*ins.state.vel; 80 | ve=ins.state.vel; 81 | if(opt.att_def==+E_AttDefination::ENU_RFU){ 82 | rpy[0]=ins.state.att[1],rpy[1]=ins.state.att[0],rpy[2]=2*PI-ins.state.att[2]; 83 | double tmp=vn[0]; 84 | vn[0]=vn[1];vn[1]=tmp;vn[2]=-vn[2]; 85 | } 86 | else{ 87 | rpy=ins.state.att; 88 | } 89 | cov_re=ins.state.cov.pos; 90 | cov_ve=ins.state.cov.vel; 91 | cov_rpy=ins.state.cov.att; 92 | } 93 | else if(opt.nav_coord==+E_InsNavCoord::LLH){ 94 | rn=ins.state.pos,vn=ins.state.vel; 95 | re=Coordinate::llh2ecef(rn); 96 | Matrix3d Cne=Rotation::getCne(rn,opt.att_def); 97 | ve=Cne*vn; 98 | if(opt.att_def==+E_AttDefination::ENU_RFU){ 99 | rpy[0]=ins.state.att[1],rpy[1]=ins.state.att[0],rpy[2]=2*PI-ins.state.att[2]; 100 | vn[0]=ins.state.vel[1],vn[1]=ins.state.vel[0],vn[2]=-ins.state.vel[2]; 101 | } 102 | else{ 103 | rpy=ins.state.att; 104 | } 105 | 106 | cov_re=Cne*(ins.earth.Mpv*ins.state.cov.pos*ins.earth.Mpv.transpose())*Cne.transpose(); 107 | cov_ve=Cne*ins.state.cov.vel*Cne.transpose(); 108 | cov_rpy=ins.state.cov.att; 109 | } 110 | 111 | sol.stat=prc_mode._value+6; 112 | 113 | sol.gstat=ins.gstat; 114 | sol.ns=ins.ns; 115 | sol.time=gpst2time(week,ins.state.sow); 116 | 117 | matcpy(sol.rr,re.data(),3,1); 118 | for(int i=0;i<3;i++) sol.qr[i]=(float)cov_re(i,i); 119 | sol.qr[3]=(float)cov_re(1,0); 120 | sol.qr[4]=(float)cov_re(2,1); 121 | sol.qr[5]=(float)cov_re(2,0); 122 | 123 | matcpy(sol.rr+3,ve.data(),3,1); 124 | for(int i=0;i<3;i++) sol.qv[i]=(float)cov_ve(i,i); 125 | sol.qv[3]=(float)cov_ve(1,0); 126 | sol.qv[4]=(float)cov_ve(2,1); 127 | sol.qv[5]=(float)cov_ve(2,0); 128 | 129 | matcpy(sol.rpy,rpy.data(),3,1); 130 | for(int i=0;i<3;i++) sol.qa[i]=(float)cov_rpy(i,i); 131 | sol.qa[3]=(float)cov_rpy(1,0); 132 | sol.qa[4]=(float)cov_rpy(2,1); 133 | sol.qa[5]=(float)cov_rpy(2,0); 134 | 135 | matcpy(sol.bg,ins.state.bg.data(),3,1); 136 | matcpy(sol.ba,ins.state.ba.data(),3,1); 137 | matcpy(sol.sg,ins.state.sg.data(),3,1); 138 | matcpy(sol.sa,ins.state.sa.data(),3,1); 139 | } 140 | 141 | /* sqrt of covariance --------------------------------------------------------*/ 142 | static double sqvar(double covar) 143 | { 144 | return covar<0.0?-sqrt(-covar):sqrt(covar); 145 | } 146 | 147 | /* solution option to field separator ----------------------------------------*/ 148 | static const char *opt2sep(const solopt_t *opt) 149 | { 150 | if (!*opt->sep) return " "; 151 | else if (!strcmp(opt->sep,"\\t")) return "\t"; 152 | return opt->sep; 153 | } 154 | 155 | extern int outInsState(uint8_t *buff,const char *s,const sol_t *sol, const solopt_t *opt) 156 | { 157 | const char *sep=opt2sep(opt); 158 | char *p=(char *)buff; 159 | 160 | trace(5,"outecef:\n"); 161 | 162 | p+=sprintf(p,"%s%3d%s%10.5f%s%10.5f%s%10.5f%s%9.5f%s%8.5f%s%8.5f%s%8.5f%s" 163 | "%8.5f%s%8.5f", 164 | sep,sol->gstat,sep,sol->rpy[0]*R2D,sep,sol->rpy[1]*R2D,sep,sol->rpy[2]*R2D,sep, 165 | SQRT(sol->qa[0])*R2D,sep,SQRT(sol->qa[1])*R2D,sep,SQRT(sol->qa[2])*R2D, 166 | sep,sqvar(sol->qa[3])*R2D,sep,sqvar(sol->qa[4])*R2D,sep, 167 | sqvar(sol->qa[5])*R2D); 168 | 169 | p+= sprintf(p,"%s%8.3f%s%8.3f%s%8.3f",sep,sol->ba[0]*MPS22MG,sep,sol->ba[1]*MPS22MG,sep,sol->ba[2]*MPS22MG); 170 | 171 | p+= sprintf(p,"%s%10.3f%s%10.3f%s%10.3f",sep,sol->bg[0]*RPS2DPH,sep,sol->bg[1]*RPS2DPH,sep,sol->bg[2]*RPS2DPH); 172 | 173 | p+= sprintf(p,"%s%8.3f%s%8.3f%s%8.3f",sep,sol->sg[0]*SCALE2PPM,sep,sol->sg[1]*SCALE2PPM,sep,sol->sg[2]*SCALE2PPM); 174 | 175 | p+= sprintf(p,"%s%8.3f%s%8.3f%s%8.3f",sep,sol->sa[0]*SCALE2PPM,sep,sol->sa[1]*SCALE2PPM,sep,sol->sa[2]*SCALE2PPM); 176 | 177 | #if 0 178 | p+= sprintf(p,"%s%8.3f%s%8.3f%s%8.3f",sep,sol->sa[0]*SCALE2PPM,sep,sol->sa[1]*SCALE2PPM,sep,sol->sa[2]*SCALE2PPM); 179 | 180 | p+= sprintf(p,"%s%8.3f%s%8.3f%s%8.3f",sep,sol->sg[0]*SCALE2PPM,sep,sol->sg[1]*SCALE2PPM,sep,sol->sg[2]*SCALE2PPM); 181 | #endif 182 | return (int)(p-(char *)buff); 183 | } 184 | -------------------------------------------------------------------------------- /src/misc/OutSol.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 10/16/22. 3 | // 4 | 5 | #ifndef FUSING_OUTSOL_H 6 | #define FUSING_OUTSOL_H 7 | 8 | #include "Fusing.h" 9 | 10 | extern int outInsState(uint8_t *buff,const char *s,const sol_t *sol, const solopt_t *opt); 11 | extern void state2sol(const imuopt_t opt,const fusing_t &fusing,const ins_t &ins,sol_t &sol,int week,E_PrcMode prc_mode); 12 | extern void writeSol(FILE *fp,const state_t &state,E_InsNavCoord mech_coord,E_AttDefination att_type,bool tc,const sol_t &sol); 13 | extern void writeSol1(FILE *fp,const sol_t& sol); 14 | 15 | #endif //FUSING_OUTSOL_H 16 | -------------------------------------------------------------------------------- /src/misc/RefSol.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 10/11/22. 3 | // 4 | 5 | #include "RefSol.h" 6 | #include "Rotation.h" 7 | #include "Coordinate.h" 8 | 9 | static int buff2num(char *buff, const char *sep, double *v) 10 | { 11 | int n,len=(int)strlen(sep); 12 | char *p,*q; 13 | char bu[MAXSOLMSG]; 14 | 15 | strcpy(bu,buff); 16 | 17 | for (p=bu,n=0;n<1024;p=q+len) { 18 | if ((q=strstr(p,sep))) *q='\0'; 19 | if (*p) v[n++]=atof(p); 20 | if (!q) break; 21 | } 22 | return n; 23 | } 24 | 25 | static bool loadRefGins(FILE *fp,solbuf_t *ref_sol) 26 | { 27 | int week,n; 28 | double sow; 29 | Vector3d rn,vn,re,ve,att,ba,bg; 30 | Matrix3d Cne; 31 | sol_t data; 32 | char line[512]; 33 | 34 | while(fgets(line,512,fp)){ 35 | if(line[0]=='%') continue; 36 | n=sscanf(line,"%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", 37 | &week,&sow,&rn.x(),&rn.y(),&rn.z(),&vn.x(),&vn.y(),&vn.z(),&att.x(),&att.y(),&att.z(), 38 | &bg.x(),&bg.y(),&bg.z(),&ba.x(),&ba.y(),&ba.z()); 39 | 40 | data.time= gpst2time(week,sow); 41 | rn[0]*=D2R,rn[1]*=D2R; 42 | re=Coordinate::llh2ecef(rn); 43 | Cne=Rotation::getCne(rn,E_AttDefination::NED_FRD); 44 | ve=Cne*vn; 45 | data.rr[0]=re[0],data.rr[1]=re[1],data.rr[2]=re[2]; 46 | data.rr[3]=ve[0],data.rr[4]=ve[1],data.rr[5]=ve[2]; 47 | data.rpy[0]=att[0]*D2R,data.rpy[1]=att[1]*D2R,data.rpy[2]=att[2]*D2R; 48 | if(n>11){ 49 | data.bg[0]=bg[0],data.bg[1]=bg[1],data.bg[2]=bg[2]; 50 | data.ba[0]=ba[0],data.ba[1]=ba[1],data.ba[2]=ba[2]; 51 | } 52 | if(!addsol(ref_sol,&data)){ 53 | return false; 54 | } 55 | } 56 | 57 | return true; 58 | } 59 | 60 | static bool loadRefIE(FILE *fp,solbuf_t *ref_sol) 61 | { 62 | int week,n,i=2; 63 | double sow; 64 | Vector3d rn,vn,att,pos; 65 | sol_t data; 66 | char line[512]; 67 | double v[50]; 68 | 69 | while(fgets(line,512,fp)){ 70 | if(48>=line[1]||line[1]>=57) { 71 | continue; 72 | } 73 | if((n= buff2num(line,",",v))<3){ 74 | return false; 75 | } 76 | data.time= gpst2time(v[0],v[1]); 77 | if(i+3n>0; 130 | } 131 | 132 | extern bool loadRef(string ref_file,E_RefSolFmt fmt,solbuf_t *ref_sol) 133 | { 134 | if(ref_file.empty()){ 135 | LOG(ERROR)<<"ref file error"; 136 | return false; 137 | } 138 | 139 | bool stat=false; 140 | FILE *fp; 141 | if(!(fp=fopen(ref_file.c_str(),"r"))){ 142 | LOG(ERROR)<<"GNSS solution file open error"; 143 | return false; 144 | } 145 | 146 | switch (fmt) { 147 | case +E_RefSolFmt::IE: 148 | stat = loadRefIE(fp,ref_sol); 149 | break; 150 | case +E_RefSolFmt::RTKLIB: 151 | stat = loadRtklibSol(fp,ref_sol); 152 | break; 153 | case +E_RefSolFmt::PSINS: 154 | break; 155 | case +E_RefSolFmt::GINS: 156 | stat = loadRefGins(fp,ref_sol); 157 | break; 158 | case +E_RefSolFmt::FUSING: 159 | break; 160 | } 161 | return stat; 162 | } 163 | 164 | static void avpinterp(Vector3d delta,double dt,double all_dt,Vector3d& avp,bool att) 165 | { 166 | if(att){ 167 | for(int j=0;j<3;j++){ 168 | if(delta[j]>PI*R2D){ 169 | delta[j]-=2*PI*R2D; 170 | } 171 | if(delta[j]<-PI*R2D){ 172 | delta[j]+=2*PI*R2D; 173 | } 174 | } 175 | } 176 | avp+=delta*(dt/all_dt); 177 | 178 | } 179 | 180 | extern bool matchRefSol(double sow,int *idx,const track_t *ref_sols,state_t *ref_data,bool back,int hz) 181 | { 182 | double ref_time,ref_time1,dt0,dt1,dd,delta_t=1.0/hz; 183 | Vector3d delta; 184 | double dt_ref= ref_sols->data[2].state.sow-ref_sols->data[1].state.sow; 185 | 186 | if(back){ 187 | for(int i=*idx;i>1;i--){ 188 | ref_time=ref_sols->data[i].state.sow; 189 | ref_time1=ref_sols->data[i-1].state.sow; 190 | if(sow<=ref_time&&sow>ref_time1){ 191 | dt0=fabs(sow-ref_time); 192 | dt1=fabs(ref_time1-sow); 193 | dt0data[*idx].state; 195 | dt0data[i].pos-ref_sols->data[i-1].pos; 198 | // avpinterp(delta,-dd, fabs(ref_time-ref_time1),ref_data->pos,false); 199 | // delta=ref_sols->data[i].vel-ref_sols->data[i-1].vel; 200 | // avpinterp(delta,-dd, fabs(ref_time-ref_time1),ref_data->vel,false); 201 | // delta=ref_sols->data[i].att-ref_sols->data[i-1].att; 202 | // avpinterp(delta,dd, fabs(ref_time-ref_time1),ref_data->att,true); 203 | return true; 204 | } 205 | else if(ref_time-sow<-1.0) break; 206 | } 207 | } 208 | else{ 209 | for(int i=*idx>5?*idx-3:*idx;in;i++){ 210 | ref_time=ref_sols->data[i].state.sow; 211 | 212 | if(ref_time-sow<-1.0){ 213 | i++; 214 | } 215 | if(ref_time-sow>60.0){ 216 | break; 217 | } 218 | if(fabs(sow-ref_time)data[i].state; 221 | return true; 222 | } 223 | } 224 | } 225 | 226 | return false; 227 | } 228 | 229 | static int matchRefSolIdx(gtime_t solTime,const solbuf_t *refSol,int *refIdx,int dir){ 230 | int i,stat=0; 231 | double dtRef=timediff(refSol->data[2].time,refSol->data[1].time); 232 | 233 | double secSol=time2gpst(solTime,NULL); 234 | double secRef; 235 | 236 | if(dir==0){ 237 | for(i=*refIdx>100?*refIdx-100:*refIdx;in;i++){ 238 | secRef=time2gpst(refSol->data[i].time,NULL); 239 | if(secRef-secSol<-1.0){ 240 | i++; 241 | } 242 | if(secRef-secSol>60.0){ 243 | break; 244 | } 245 | if(secSol>=secRef&&secSol-secRefn)?*refIdx-100:*refIdx;i>0;i--){ 258 | secRef=time2gpst(refSol->data[i].time,NULL); 259 | if(secRef-secSol<-1.0){ 260 | i--; 261 | } 262 | if(secRef-secSol>60.0){ 263 | // break; 264 | continue; 265 | } 266 | if(fabs(secSol-secRef)data[0].time,sol->data[1].time)>0.0) dir=1,refIdx=refSol->n-1; 289 | 290 | errSol->err_fmt=1; 291 | for(i=0;in;i++){ 292 | if(matchRefSolIdx(sol->data[i].time,refSol,&refIdx,dir)){ 293 | solData=sol->data[i]; 294 | for(int j=0;j<6;j++){ 295 | solData.rr[j]=sol->data[i].rr[j]-refSol->data[refIdx].rr[j]; 296 | } 297 | double pos[3]; 298 | ecef2pos(refSol->data[refIdx].rr,pos); 299 | ecef2enu(pos,solData.rr,solData.rr); 300 | ecef2enu(pos,solData.rr+3,solData.rr+3); /*rr enu format*/ 301 | sow= time2gpst(solData.time,NULL); 302 | #if 0 303 | quat_t q1,q2,dq; 304 | rpy2quat(sol->data[i].att,&q1); 305 | rpy2quat(refSol->data[refIdx].att,&q2); 306 | quat_conj(&q2); 307 | dq=quatMul(&q1,&q2); 308 | quat2rpy(&dq,solData.att); 309 | #else 310 | solData.rpy[0]=(sol->data[i].rpy[0])-(refSol->data[refIdx].rpy[0]); 311 | solData.rpy[1]=(sol->data[i].rpy[1])-(refSol->data[refIdx].rpy[1]); 312 | solData.rpy[2]=sol->data[i].rpy[2]-refSol->data[refIdx].rpy[2]; 313 | if(solData.rpy[2]>350*D2R) solData.rpy[2]-=2*PI; 314 | if(solData.rpy[2]<-350*D2R) solData.rpy[2]+=2*PI; 315 | for(int j=0;j<6;j++){ 316 | solData.qr[j]=sol->data[i].qr[j]; 317 | solData.qv[j]=sol->data[i].qv[j]; 318 | solData.qa[j]=sol->data[i].qa[j]; 319 | } 320 | #endif 321 | if(!addsol(errSol,&solData)){ 322 | return 0; 323 | } 324 | } 325 | } 326 | if(refIdx==0) return 0; 327 | 328 | return 1; 329 | } -------------------------------------------------------------------------------- /src/misc/RefSol.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 10/11/22. 3 | // 4 | 5 | #ifndef FUSING_REFSOL_H 6 | #define FUSING_REFSOL_H 7 | 8 | #include "EigenInc.h" 9 | #include "Logger.h" 10 | #include "Enums.h" 11 | #include "Imu.h" 12 | #include "Gnss.h" 13 | #include "Fusing.h" 14 | 15 | 16 | extern bool loadRef(string ref_file,E_RefSolFmt fmt,solbuf_t *ref_sol); 17 | extern bool matchRefSol(double sow,int *idx,const track_t *ref_sols,state_t *ref_data,bool back,int hz); 18 | extern int makeSolDiff(const solbuf_t *sol,const solbuf_t *refSol,solbuf_t *errSol); 19 | #endif //FUSING_REFSOL_H 20 | -------------------------------------------------------------------------------- /src/sensors/gnss/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.0) 2 | 3 | project(${FGnssLib}) 4 | 5 | file(GLOB_RECURSE header_files ${libFGnssSrc}/*.h ${libFGnssSrc}/rtklib/*.h) 6 | file(GLOB_RECURSE source_files ${libFGnssSrc}/*.cc ${libFGnssSrc}/rtklib/*.cc ${libFGnssSrc}/rtklib/rcv/*.cc) 7 | 8 | include_directories(${libFGnssSrc} 9 | ${libFGnssSrc}/rtklib 10 | ${libCommonSrc} 11 | ${libFusingSrc} 12 | ${libMiscSrc}) 13 | 14 | add_library(${PROJECT_NAME} ${header_files} ${source_files}) 15 | 16 | target_link_libraries(${PROJECT_NAME} ${CommonLib} ${FusingLib} ${MiscLib}) 17 | 18 | if (CMAKE_SYSTEM_NAME MATCHES "Windows") 19 | link_directories(${ROOT}/build/Lib/Debug) 20 | link_directories(${ROOT}/build/Lib/Release) 21 | link_directories(${ROOT}/build/Lib/RelWithDebInfo) 22 | link_directories(${ROOT}/build/Lib/MinSizeRel) 23 | else () 24 | link_directories(${ROOT}/build/Lib) 25 | endif () 26 | -------------------------------------------------------------------------------- /src/sensors/gnss/Gnss.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 10/2/22. 3 | // 4 | 5 | #include "Coordinate.h" 6 | #include "Rotation.h" 7 | #include "Gnss.h" 8 | #include "rtklib.h" 9 | 10 | /* safe to call malloc */ 11 | #define MALLOC(pointer, type, sz) \ 12 | if(!((pointer) = (type *)malloc(((unsigned long)sz)*sizeof(type)))){ \ 13 | LOG(ERROR)<<" memory allocation failed"; \ 14 | } 15 | 16 | /* safe to free pointer */ 17 | #define FREE(pointer) {free(pointer);(pointer) = NULL;} 18 | 19 | static bool addPvData(const pvData_t *pv_data,pv_t *pvs) { 20 | pvData_t *data_tmp; 21 | 22 | if(pvs->n==0&&pvs->nmax==0){ 23 | pvs->nmax=10000; 24 | MALLOC(pvs->data,pvData_t ,pvs->nmax); 25 | } 26 | else if(pvs->n>=pvs->nmax){ 27 | pvs->nmax*=2; 28 | if(!(data_tmp=(pvData_t *)realloc(pvs->data, sizeof(pvData_t)*pvs->nmax))){ 29 | FREE(pvs->data); 30 | pvs->n=pvs->nmax=0; 31 | return false; 32 | } 33 | pvs->data=data_tmp; 34 | } 35 | pvs->data[pvs->n++]=*pv_data; 36 | return true; 37 | } 38 | 39 | extern void sol2pvData(sol_t &sol,pvData_t &pv,E_InsNavCoord nav_coord,E_AttDefination att_def) 40 | { 41 | pv.state=sol.stat; 42 | pv.sow= time2gpst(sol.time, nullptr); 43 | pv.pos=(Eigen::Map)(sol.rr); 44 | pv.vel=(Eigen::Map)(sol.rr+3); 45 | for(int j=0;j<3;j++){ 46 | pv.cov_pos[j]=(double)sol.qr[j]; 47 | pv.cov_vel[j]=(double)sol.qv[j]; 48 | } 49 | if(nav_coord==+E_InsNavCoord::LLH){ 50 | Vector3d llh=Coordinate::ecef2llh(pv.pos); 51 | pv.pos=llh; 52 | Matrix3d Cne=Rotation::getCne(llh,att_def); 53 | Matrix3d R=pv.cov_pos.asDiagonal(); 54 | R=Cne.transpose()*R*Cne; 55 | pv.cov_pos[0]=R(0,0)/(RE_WGS84*RE_WGS84); 56 | pv.cov_pos[1]=R(1,1)/(RE_WGS84*RE_WGS84); 57 | pv.cov_pos[2]=R(2,2); 58 | pv.vel=Cne.transpose()*pv.vel; 59 | } 60 | } 61 | 62 | static bool readPvPos(FILE *fp,pv_t *pvs,E_InsNavCoord nav_coord,E_AttDefination att_def) 63 | { 64 | solopt_t opt=solopt_default; 65 | solbuf_t solbuf={0}; 66 | gtime_t t0={0}; 67 | 68 | initsolbuf(&solbuf,0,0); 69 | readsolopt(fp,&opt); 70 | rewind(fp); 71 | 72 | if(!readsoldata(fp,t0,t0,0.0,0,&opt,&solbuf)){ 73 | LOG(ERROR)<<"read rtklib pos file error"; 74 | return false; 75 | } 76 | 77 | pvData_t data; 78 | for(int i=0;i labels according GPX specs 12 | *-----------------------------------------------------------------------------*/ 13 | #include "rtklib.h" 14 | 15 | /* constants -----------------------------------------------------------------*/ 16 | 17 | #define HEADXML "\n" 18 | #define HEADGPX "\n" 19 | #define TAILGPX "" 20 | 21 | static const char *XMLNS="http://www.topografix.com/GPX/1/1"; 22 | 23 | /* output waypoint -----------------------------------------------------------*/ 24 | static void outpoint(FILE *fp, gtime_t time, const double *pos, 25 | const char *label, int stat, int outalt, int outtime) 26 | { 27 | /* fix, float, sbas and ppp are rtklib extentions to GPX */ 28 | const char *fix_label[]={"fix","float","sbas","dgps","3d","ppp"}; 29 | double ep[6]; 30 | 31 | fprintf(fp,"\n",pos[0]*R2D,pos[1]*R2D); 32 | if (outalt) { 33 | fprintf(fp," %.4f\n",pos[2]-(outalt==2?geoidh(pos):0.0)); 34 | } 35 | if (outtime) { 36 | if (outtime==2) time=gpst2utc(time); 37 | else if (outtime==3) time=timeadd(gpst2utc(time),9*3600.0); 38 | time2epoch(time,ep); 39 | fprintf(fp," \n", 40 | ep[0],ep[1],ep[2],ep[3],ep[4],ep[5]); 41 | } 42 | if (outalt==2) { 43 | fprintf(fp," %.4f\n",geoidh(pos)); 44 | } 45 | if (stat>=1&&stat<=6) { 46 | fprintf(fp," %s\n",fix_label[stat-1]); 47 | } 48 | if (*label) { 49 | fprintf(fp," %s\n",label); 50 | } 51 | fprintf(fp,"\n"); 52 | } 53 | /* output track --------------------------------------------------------------*/ 54 | static void outtrack(FILE *fp, const solbuf_t *solbuf, int outalt, int outtime) 55 | { 56 | gtime_t time; 57 | double pos[3],ep[6]; 58 | int i; 59 | 60 | fprintf(fp,"\n"); 61 | fprintf(fp," \n"); 62 | for (i=0;in;i++) { 63 | ecef2pos(solbuf->data[i].rr,pos); 64 | fprintf(fp," \n",pos[0]*R2D, 65 | pos[1]*R2D); 66 | if (outalt) { 67 | fprintf(fp," %.4f\n",pos[2]-(outalt==2?geoidh(pos):0.0)); 68 | } 69 | if (outtime) { 70 | time=solbuf->data[i].time; 71 | if (outtime==2) time=gpst2utc(time); 72 | else if (outtime==3) time=timeadd(gpst2utc(time),9*3600.0); 73 | time2epoch(time,ep); 74 | fprintf(fp," \n", 75 | ep[0],ep[1],ep[2],ep[3],ep[4],ep[5]); 76 | } 77 | if (outalt==2) { 78 | fprintf(fp," %.4f\n",geoidh(pos)); 79 | } 80 | fprintf(fp," \n"); 81 | } 82 | fprintf(fp," \n"); 83 | fprintf(fp,"\n"); 84 | } 85 | /* save gpx file -------------------------------------------------------------*/ 86 | static int savegpx(const char *file, const solbuf_t *solbuf, int outtrk, 87 | int outpnt, int outalt, int outtime) 88 | { 89 | FILE *fp; 90 | double pos[3]; 91 | int i; 92 | 93 | if (!(fp=fopen(file,"w"))) { 94 | fprintf(stderr,"file open error : %s\n",file); 95 | return 0; 96 | } 97 | fprintf(fp,HEADXML); 98 | fprintf(fp,HEADGPX,"RTKLIB " VER_RTKLIB,XMLNS); 99 | 100 | /* output waypoint */ 101 | if (outpnt) { 102 | for (i=0;in;i++) { 103 | ecef2pos(solbuf->data[i].rr,pos); 104 | outpoint(fp,solbuf->data[i].time,pos,"",solbuf->data[i].stat,outalt, 105 | outtime); 106 | } 107 | } 108 | /* output waypoint of ref position */ 109 | if (norm(solbuf->rb,3)>0.0) { 110 | ecef2pos(solbuf->rb,pos); 111 | outpoint(fp,solbuf->data[0].time,pos,"Reference Position",0,outalt,0); 112 | } 113 | /* output track */ 114 | if (outtrk) { 115 | outtrack(fp,solbuf,outalt,outtime); 116 | } 117 | fprintf(fp,"%s\n",TAILGPX); 118 | fclose(fp); 119 | return 1; 120 | } 121 | /* convert to GPX file --------------------------------------------------------- 122 | * convert solutions to GPX file [1] 123 | * args : char *infile I input solutions file 124 | * char *outfile I output google earth kml file ("":.kml) 125 | * gtime_t ts,te I start/end time (gpst) 126 | * int tint I time interval (s) (0.0:all) 127 | * int qflg I quality flag (0:all) 128 | * double *offset I add offset {east,north,up} (m) 129 | * int outtrk I output track (0:off,1:on) 130 | * int outpnt I output waypoint (0:off,1:on) 131 | * int outalt I output altitude (0:off,1:elipsoidal,2:geodetic) 132 | * int outtime I output time (0:off,1:gpst,2:utc,3:jst) 133 | * return : status (0:ok,-1:file read,-2:file format,-3:no data,-4:file write) 134 | *-----------------------------------------------------------------------------*/ 135 | extern int convgpx(const char *infile, const char *outfile, gtime_t ts, 136 | gtime_t te, double tint, int qflg, double *offset, 137 | int outtrk, int outpnt, int outalt, int outtime) 138 | { 139 | solbuf_t solbuf={0}; 140 | double rr[3]={0},pos[3],dr[3]; 141 | int i,j; 142 | char *p,file[1024],infile_[1024]; 143 | 144 | trace(3,"convgpx : infile=%s outfile=%s\n",infile,outfile); 145 | 146 | strcpy(infile_,infile); 147 | if (!*outfile) { 148 | if ((p=strrchr(infile_,'.'))) { 149 | strncpy(file,infile,p-infile); 150 | strcpy(file+(p-infile),".gpx"); 151 | } 152 | else sprintf(file,"%s.gpx",infile); 153 | } 154 | else strcpy(file,outfile); 155 | 156 | /* read solution file */ 157 | if (!readsolt((char **)&infile,1,ts,te,tint,qflg,&solbuf)) return -1; 158 | 159 | /* mean position */ 160 | for (i=0;i<3;i++) { 161 | for (j=0;j0.0) { 171 | for (i=0;i<3;i++) solbuf.rb[i]+=dr[i]; 172 | } 173 | /* save gpx file */ 174 | return savegpx(file,&solbuf,outtrk,outpnt,outalt,outtime)?0:-4; 175 | } 176 | 177 | -------------------------------------------------------------------------------- /src/sensors/gnss/rtklib/convkml.cc: -------------------------------------------------------------------------------- 1 | /*------------------------------------------------------------------------------ 2 | * convkml.c : google earth kml converter 3 | * 4 | * Copyright (C) 2007-2017 by T.TAKASU, All rights reserved. 5 | * 6 | * references : 7 | * [1] Open Geospatial Consortium Inc., OGC 07-147r2, OGC(R) KML, 2008-04-14 8 | * 9 | * version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $ 10 | * history : 2007/01/20 1.0 new 11 | * 2007/03/15 1.1 modify color sequence 12 | * 2007/04/03 1.2 add geodetic height option 13 | * support input of NMEA GGA sentence 14 | * delete altitude info for track 15 | * add time stamp option 16 | * separate readsol.c file 17 | * 2009/01/19 1.3 fix bug on display mark with by-q-flag option 18 | * 2010/05/10 1.4 support api readsolt() change 19 | * 2010/08/14 1.5 fix bug on readsolt() (2.4.0_p3) 20 | * 2017/06/10 1.6 support wild-card in input file 21 | *-----------------------------------------------------------------------------*/ 22 | #include "rtklib.h" 23 | 24 | /* constants -----------------------------------------------------------------*/ 25 | 26 | #define SIZP 0.2 /* mark size of rover positions */ 27 | #define SIZR 0.3 /* mark size of reference position */ 28 | #define TINT 60.0 /* time label interval (sec) */ 29 | 30 | static const char *head1=""; 31 | static const char *head2=""; 32 | static const char *mark="http://maps.google.com/mapfiles/kml/pal2/icon18.png"; 33 | 34 | /* output track --------------------------------------------------------------*/ 35 | static void outtrack(FILE *f, const solbuf_t *solbuf, const char *color, 36 | int outalt, int outtime) 37 | { 38 | double pos[3]; 39 | int i; 40 | 41 | fprintf(f,"\n"); 42 | fprintf(f,"Rover Track\n"); 43 | fprintf(f,"\n"); 48 | fprintf(f,"\n"); 49 | if (outalt) fprintf(f,"absolute\n"); 50 | fprintf(f,"\n"); 51 | for (i=0;in;i++) { 52 | ecef2pos(solbuf->data[i].rr,pos); 53 | if (outalt==0) pos[2]=0.0; 54 | else if (outalt==2) pos[2]-=geoidh(pos); 55 | fprintf(f,"%13.9f,%12.9f,%5.3f\n",pos[1]*R2D,pos[0]*R2D,pos[2]); 56 | } 57 | fprintf(f,"\n"); 58 | fprintf(f,"\n"); 59 | fprintf(f,"\n"); 60 | } 61 | /* output point --------------------------------------------------------------*/ 62 | static void outpoint(FILE *fp, gtime_t time, const double *pos, 63 | const char *label, int style, int outalt, int outtime) 64 | { 65 | double ep[6],alt=0.0; 66 | char str[256]=""; 67 | 68 | fprintf(fp,"\n"); 69 | if (*label) fprintf(fp,"%s\n",label); 70 | fprintf(fp,"#P%d\n",style); 71 | if (outtime) { 72 | if (outtime==2) time=gpst2utc(time); 73 | else if (outtime==3) time=timeadd(gpst2utc(time),9*3600.0); 74 | time2epoch(time,ep); 75 | if (!*label&&fmod(ep[5]+0.005,TINT)<0.01) { 76 | sprintf(str,"%02.0f:%02.0f",ep[3],ep[4]); 77 | fprintf(fp,"%s\n",str); 78 | } 79 | sprintf(str,"%04.0f-%02.0f-%02.0fT%02.0f:%02.0f:%05.2fZ", 80 | ep[0],ep[1],ep[2],ep[3],ep[4],ep[5]); 81 | fprintf(fp,"%s\n",str); 82 | } 83 | fprintf(fp,"\n"); 84 | if (outalt) { 85 | fprintf(fp,"1\n"); 86 | fprintf(fp,"absolute\n"); 87 | alt=pos[2]-(outalt==2?geoidh(pos):0.0); 88 | } 89 | fprintf(fp,"%13.9f,%12.9f,%5.3f\n",pos[1]*R2D, 90 | pos[0]*R2D,alt); 91 | fprintf(fp,"\n"); 92 | fprintf(fp,"\n"); 93 | } 94 | /* save kml file -------------------------------------------------------------*/ 95 | static int savekml(const char *file, const solbuf_t *solbuf, int tcolor, 96 | int pcolor, int outalt, int outtime) 97 | { 98 | FILE *fp; 99 | double pos[3]; 100 | int i,qcolor[]={0,1,2,5,4,3,0}; 101 | char *color[]={ 102 | "ffffffff","ff008800","ff00aaff","ff0000ff","ff00ffff","ffff00ff" 103 | }; 104 | if (!(fp=fopen(file,"w"))) { 105 | fprintf(stderr,"file open error : %s\n",file); 106 | return 0; 107 | } 108 | fprintf(fp,"%s\n%s\n",head1,head2); 109 | fprintf(fp,"\n"); 110 | for (i=0;i<6;i++) { 111 | fprintf(fp,"\n"); 118 | } 119 | if (tcolor>0) { 120 | outtrack(fp,solbuf,color[tcolor-1],outalt,outtime); 121 | } 122 | if (pcolor>0) { 123 | fprintf(fp,"\n"); 124 | fprintf(fp," Rover Position\n"); 125 | for (i=0;in;i++) { 126 | ecef2pos(solbuf->data[i].rr,pos); 127 | outpoint(fp,solbuf->data[i].time,pos,"", 128 | pcolor==5?qcolor[solbuf->data[i].stat]:pcolor-1,outalt,outtime); 129 | } 130 | fprintf(fp,"\n"); 131 | } 132 | if (norm(solbuf->rb,3)>0.0) { 133 | ecef2pos(solbuf->rb,pos); 134 | outpoint(fp,solbuf->data[0].time,pos,"Reference Position",0,outalt,0); 135 | } 136 | fprintf(fp,"\n"); 137 | fprintf(fp,"\n"); 138 | fclose(fp); 139 | return 1; 140 | } 141 | /* convert to google earth kml file -------------------------------------------- 142 | * convert solutions to google earth kml file 143 | * args : char *infile I input solutions file (wild-card (*) is expanded) 144 | * char *outfile I output google earth kml file ("":.kml) 145 | * gtime_t ts,te I start/end time (gpst) 146 | * int tint I time interval (s) (0.0:all) 147 | * int qflg I quality flag (0:all) 148 | * double *offset I add offset {east,north,up} (m) 149 | * int tcolor I track color 150 | * (0:none,1:white,2:green,3:orange,4:red,5:yellow) 151 | * int pcolor I point color 152 | * (0:none,1:white,2:green,3:orange,4:red,5:by qflag) 153 | * int outalt I output altitude (0:off,1:elipsoidal,2:geodetic) 154 | * int outtime I output time (0:off,1:gpst,2:utc,3:jst) 155 | * return : status (0:ok,-1:file read,-2:file format,-3:no data,-4:file write) 156 | * notes : see ref [1] for google earth kml file format 157 | *-----------------------------------------------------------------------------*/ 158 | extern int convkml(const char *infile, const char *outfile, gtime_t ts, 159 | gtime_t te, double tint, int qflg, double *offset, 160 | int tcolor, int pcolor, int outalt, int outtime) 161 | { 162 | solbuf_t solbuf={0}; 163 | double rr[3]={0},pos[3],dr[3]; 164 | int i,j,nfile,stat; 165 | char *p,file[1024],*files[MAXEXFILE]={0},infile_[1024]; 166 | 167 | trace(3,"convkml : infile=%s outfile=%s\n",infile,outfile); 168 | 169 | /* expand wild-card of infile */ 170 | for (i=0;i=0;i--) free(files[i]); 173 | return -4; 174 | } 175 | } 176 | if ((nfile=expath(infile,files,MAXEXFILE))<=0) { 177 | for (i=0;i0.0) { 211 | for (i=0;i<3;i++) solbuf.rb[i]+=dr[i]; 212 | } 213 | /* save kml file */ 214 | return savekml(file,&solbuf,tcolor,pcolor,outalt,outtime)?0:-4; 215 | } 216 | -------------------------------------------------------------------------------- /src/sensors/gnss/rtklib/datum.cc: -------------------------------------------------------------------------------- 1 | /*------------------------------------------------------------------------------ 2 | * datum.c : datum transformation 3 | * 4 | * Copyright (C) 2007 by T.TAKASU, All rights reserved. 5 | * 6 | * version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $ 7 | * history : 2007/02/08 1.0 new 8 | *-----------------------------------------------------------------------------*/ 9 | #include "rtklib.h" 10 | 11 | #define MAXPRM 400000 /* max number of parameter records */ 12 | 13 | typedef struct { /* datum trans parameter type */ 14 | int code; /* mesh code */ 15 | float db,dl; /* difference of latitude/longitude (sec) */ 16 | } tprm_t; 17 | 18 | static tprm_t *prm=NULL; /* datum trans parameter table */ 19 | static int n=0; /* datum trans parameter table size */ 20 | 21 | /* compare datum trans parameters --------------------------------------------*/ 22 | static int cmpprm(const void *p1, const void *p2) 23 | { 24 | tprm_t *q1=(tprm_t *)p1,*q2=(tprm_t *)p2; 25 | return q1->code-q2->code; 26 | } 27 | /* search datum trans parameter ----------------------------------------------*/ 28 | static int searchprm(double lat, double lon) 29 | { 30 | int i,j,k,n1,m1,n2,m2,code; 31 | 32 | lon-=6000.0; 33 | n1=(int)(lat/40.0); lat-=n1*40.0; 34 | m1=(int)(lon/60.0); lon-=m1*60.0; 35 | n2=(int)(lat/5.0); lat-=n2*5.0; 36 | m2=(int)(lon/7.5); lon-=m2*7.5; 37 | code=n1*1000000+m1*10000+n2*1000+m2*100+(int)(lat/0.5)*10+(int)(lon/0.75); 38 | 39 | for (i=0,j=n-1;i:error) 68 | * notes : parameters file shall comply with GSI TKY2JGD.par 69 | *-----------------------------------------------------------------------------*/ 70 | extern int loaddatump(const char *file) 71 | { 72 | FILE *fp; 73 | char buff[256]; 74 | 75 | if (n>0) return 0; /* already loaded */ 76 | 77 | if (!(fp=fopen(file,"r"))) { 78 | fprintf(stderr,"%s : datum prm file open error : %s\n",__FILE__,file); 79 | return -1; 80 | } 81 | if (!(prm=(tprm_t *)malloc(sizeof(tprm_t)*MAXPRM))) { 82 | fprintf(stderr,"%s : memory allocation error\n",__FILE__); 83 | return -1; 84 | } 85 | while (fgets(buff,sizeof(buff),fp)&&n=3) n++; 87 | } 88 | fclose(fp); 89 | qsort(prm,n,sizeof(tprm_t),cmpprm); /* sort parameter table */ 90 | return 0; 91 | } 92 | /* tokyo datum to JGD2000 datum ------------------------------------------------ 93 | * transform position in Tokyo datum to JGD2000 datum 94 | * args : double *pos I position in Tokyo datum {lat,lon,h} (rad,m) 95 | * O position in JGD2000 datum {lat,lon,h} (rad,m) 96 | * return : status (0:ok,0>:error,out of range) 97 | * notes : before calling, call loaddatump() to set parameter table 98 | *-----------------------------------------------------------------------------*/ 99 | extern int tokyo2jgd(double *pos) 100 | { 101 | double post[2],dpos[2]; 102 | 103 | post[0]=pos[0]; 104 | post[1]=pos[1]; 105 | if (dlatdlon(post,dpos)) return -1; 106 | pos[0]=post[0]+dpos[0]; 107 | pos[1]=post[1]+dpos[1]; 108 | return 0; 109 | } 110 | /* JGD2000 datum to Tokyo datum ------------------------------------------------ 111 | * transform position in JGD2000 datum to Tokyo datum 112 | * args : double *pos I position in JGD2000 datum {lat,lon,h} (rad,m) 113 | * O position in Tokyo datum {lat,lon,h} (rad,m) 114 | * return : status (0:ok,0>:error,out of range) 115 | * notes : before calling, call loaddatump() to set parameter table 116 | *-----------------------------------------------------------------------------*/ 117 | extern int jgd2tokyo(double *pos) 118 | { 119 | double posj[2],dpos[2]; 120 | int i; 121 | 122 | posj[0]=pos[0]; 123 | posj[1]=pos[1]; 124 | for (i=0;i<2;i++) { 125 | if (dlatdlon(pos,dpos)) return -1; 126 | pos[0]=posj[0]-dpos[0]; 127 | pos[1]=posj[1]-dpos[1]; 128 | } 129 | return 0; 130 | } 131 | -------------------------------------------------------------------------------- /src/sensors/gnss/rtklib/lambda.cc: -------------------------------------------------------------------------------- 1 | /*------------------------------------------------------------------------------ 2 | * lambda.c : integer ambiguity resolution 3 | * 4 | * Copyright (C) 2007-2008 by T.TAKASU, All rights reserved. 5 | * 6 | * reference : 7 | * [1] P.J.G.Teunissen, The least-square ambiguity decorrelation adjustment: 8 | * a method for fast GPS ambiguity estimation, J.Geodesy, Vol.70, 65-82, 9 | * 1995 10 | * [2] X.-W.Chang, X.Yang, T.Zhou, MLAMBDA: A modified LAMBDA method for 11 | * integer least-squares estimation, J.Geodesy, Vol.79, 552-565, 2005 12 | * 13 | * version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $ 14 | * history : 2007/01/13 1.0 new 15 | * 2015/05/31 1.1 add api lambda_reduction(), lambda_search() 16 | *-----------------------------------------------------------------------------*/ 17 | #include "rtklib.h" 18 | 19 | /* constants/macros ----------------------------------------------------------*/ 20 | 21 | #define LOOPMAX 10000 /* maximum count of search loop */ 22 | 23 | #define SGN(x) ((x)<=0.0?-1.0:1.0) 24 | #define ROUND(x) (floor((x)+0.5)) 25 | #define SWAP(x,y) do {double tmp_; tmp_=x; x=y; y=tmp_;} while (0) 26 | 27 | /* LD factorization (Q=L'*diag(D)*L) -----------------------------------------*/ 28 | static int LD(int n, const double *Q, double *L, double *D) 29 | { 30 | int i,j,k,info=0; 31 | double a,*A=mat(n,n); 32 | 33 | memcpy(A,Q,sizeof(double)*n*n); 34 | for (i=n-1;i>=0;i--) { 35 | if ((D[i]=A[i+i*n])<=0.0) {info=-1; break;} 36 | a=sqrt(D[i]); 37 | for (j=0;j<=i;j++) L[i+j*n]=A[i+j*n]/a; 38 | for (j=0;j<=i-1;j++) for (k=0;k<=j;k++) A[j+k*n]-=L[i+k*n]*L[i+j*n]; 39 | for (j=0;j<=i;j++) L[i+j*n]/=L[i+i*n]; 40 | } 41 | free(A); 42 | if (info) fprintf(stderr,"%s : LD factorization error\n",__FILE__); 43 | return info; 44 | } 45 | /* integer gauss transformation ----------------------------------------------*/ 46 | static void gauss(int n, double *L, double *Z, int i, int j) 47 | { 48 | int k,mu; 49 | 50 | if ((mu=(int)ROUND(L[i+j*n]))!=0) { 51 | for (k=i;k=0) { 81 | if (j<=k) for (i=j+1;is[imax]) imax=nn; 126 | for (i=0;i=LOOPMAX) { 163 | fprintf(stderr,"%s : search loop count overflow\n",__FILE__); 164 | return -2; 165 | } 166 | return 0; 167 | } 168 | /* lambda/mlambda integer least-square estimation ------------------------------ 169 | * integer least-square estimation. reduction is performed by lambda (ref.[1]), 170 | * and search by mlambda (ref.[2]). 171 | * args : int n I number of float parameters 172 | * int m I number of fixed solutions 173 | * double *a I float parameters (n x 1) (double-diff phase biases) 174 | * double *Q I covariance matrix of float parameters (n x n) 175 | * double *F O fixed solutions (n x m) 176 | * double *s O sum of squared residulas of fixed solutions (1 x m) 177 | * return : status (0:ok,other:error) 178 | * notes : matrix stored by column-major order (fortran convension) 179 | *-----------------------------------------------------------------------------*/ 180 | extern int lambda(int n, int m, const double *a, const double *Q, double *F, 181 | double *s) 182 | { 183 | int info; 184 | double *L,*D,*Z,*z,*E; 185 | 186 | if (n<=0||m<=0) return -1; 187 | L=zeros(n,n); D=mat(n,1); Z=eye(n); z=mat(n,1); E=mat(n,m); 188 | 189 | /* LD (lower diaganol) factorization (Q=L'*diag(D)*L) */ 190 | if (!(info=LD(n,Q,L,D))) { 191 | 192 | /* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) */ 193 | reduction(n,L,D,Z); 194 | matmul("TN",n,1,n,1.0,Z,a,0.0,z); /* z=Z'*a */ 195 | 196 | /* mlambda search 197 | z = transformed double-diff phase biases 198 | L,D = transformed covariance matrix */ 199 | if (!(info=search(n,m,L,D,z,E,s))) { /* returns 0 if no error */ 200 | 201 | info=solve("T",Z,E,n,m,F); /* F=Z'\E */ 202 | } 203 | } 204 | free(L); free(D); free(Z); free(z); free(E); 205 | return info; 206 | } 207 | /* lambda reduction ------------------------------------------------------------ 208 | * reduction by lambda (ref [1]) for integer least square 209 | * args : int n I number of float parameters 210 | * double *Q I covariance matrix of float parameters (n x n) 211 | * double *Z O lambda reduction matrix (n x n) 212 | * return : status (0:ok,other:error) 213 | *-----------------------------------------------------------------------------*/ 214 | extern int lambda_reduction(int n, const double *Q, double *Z) 215 | { 216 | double *L,*D; 217 | int i,j,info; 218 | 219 | if (n<=0) return -1; 220 | 221 | L=zeros(n,n); D=mat(n,1); 222 | 223 | for (i=0;itime, nullptr); 15 | for(i=0;itime, nullptr); 15 | for(i=0;isateph,rs,dts,var,svh); 56 | 57 | for(i=0;i 6 | #include 7 | #include "Fusing.h" 8 | 9 | #define MAXIMUDATA 100000 10 | #define MAXSTRLINEIMUDATA 512 11 | 12 | /* safe to call malloc */ 13 | #define MALLOC(pointer, type, sz) \ 14 | if(!((pointer) = (type *)malloc(((unsigned long)sz)*sizeof(type)))){ \ 15 | LOG(ERROR)<<" memory allocation failed"; \ 16 | } 17 | 18 | /* safe to free pointer */ 19 | #define FREE(pointer) {free(pointer);(pointer) = NULL;} 20 | 21 | static void stringSplit(const string& str,const string& split,vector& res) 22 | { 23 | std::regex reg(split); 24 | std::sregex_token_iterator pos(str.begin(),str.end(),reg,-1); 25 | decltype(pos) end; 26 | for(;pos!=end;++pos){ 27 | res.push_back(pos->str()); 28 | } 29 | } 30 | 31 | static bool addImuData(const imuDataUnit_t *imu_data,imu_t *imu) 32 | { 33 | imuDataUnit_t *data_tmp; 34 | 35 | if(imu->n==0&&imu->nmax==0){ 36 | imu->nmax=MAXIMUDATA; 37 | MALLOC(imu->data,imuDataUnit_t,imu->nmax); 38 | } 39 | else if(imu->n>=imu->nmax){ 40 | imu->nmax*=2; 41 | if(!(data_tmp=(imuDataUnit_t*) realloc(imu->data, sizeof(imuDataUnit_t)*imu->nmax))){ 42 | FREE(imu->data); 43 | imu->n=imu->nmax=0; 44 | return false; 45 | } 46 | imu->data=data_tmp; 47 | } 48 | imu->data[imu->n++]=*imu_data; 49 | return true; 50 | } 51 | 52 | static bool readImuDataNvt(double ts,double te,fstream &file_fp,imuopt_t imu_opt,imu_t *imu) 53 | { 54 | double sow,gx,gy,gz,ax,ay,az; 55 | imuDataUnit_t imu_data; 56 | string line; 57 | while(std::getline(file_fp,line)){ 58 | if(line[0]=='%') continue; 59 | sscanf(line.c_str(),"%lf %lf %lf %lf %lf %lf %lf",&sow,&gx,&gy,&gz,&ax,&ay,&az); 60 | 61 | if((ts!=0.0&&sow>=ts)&&(te!=0.0&&sow<=te)){ 62 | imu_data.sow=sow; 63 | if(imu_opt.att_def==+E_AttDefination::ENU_RFU){ 64 | imu_data.gyr=Vector3d(gx*D2R,gy*D2R,gz*D2R); 65 | imu_data.acc=Vector3d(ax,ay,az); 66 | } 67 | else{ 68 | imu_data.gyr=Vector3d(gy*D2R,gx*D2R,-gz*D2R); 69 | imu_data.acc=Vector3d(ay,ax,-az); 70 | } 71 | addImuData(&imu_data,imu); 72 | } 73 | } 74 | 75 | return true; 76 | } 77 | 78 | static bool readImuDataPsins(fstream &file_fp,imuopt_t imu_opt,imu_t *imu) 79 | { 80 | double sow,gx,gy,gz,ax,ay,az; 81 | imuDataUnit_t imu_data; 82 | string line; 83 | while(std::getline(file_fp,line)){ 84 | if(line[0]=='%') continue; 85 | sscanf(line.c_str(),"%lf,%lf,%lf,%lf,%lf,%lf,%lf",&gx,&gy,&gz,&ax,&ay,&az,&sow); 86 | 87 | imu_data.sow=sow; 88 | if(imu_opt.att_def==+E_AttDefination::ENU_RFU){ 89 | imu_data.gyr=Vector3d(gx,gy,gz); 90 | imu_data.acc=Vector3d(ax,ay,az); 91 | } 92 | else{ 93 | imu_data.gyr=Vector3d(gy,gx,-gz); 94 | imu_data.acc=Vector3d(ay,ax,-az); 95 | } 96 | imu_data.acc*=imu->property.rate; 97 | imu_data.gyr*=imu->property.rate; 98 | addImuData(&imu_data,imu); 99 | } 100 | 101 | return true; 102 | } 103 | 104 | static bool readImuDataGins(fstream &file_fp,imuopt_t imu_opt,imu_t *imu) 105 | { 106 | double sow,gx,gy,gz,ax,ay,az; 107 | imuDataUnit_t imu_data; 108 | string line; 109 | while(std::getline(file_fp,line)){ 110 | if(line[0]=='%') continue; 111 | sscanf(line.c_str(),"%lf %lf %lf %lf %lf %lf %lf",&sow,&gx,&gy,&gz,&ax,&ay,&az); 112 | 113 | imu_data.sow=sow; 114 | if(imu_opt.att_def==+E_AttDefination::ENU_RFU){ 115 | imu_data.gyr=Vector3d(gy,gx,-gz); 116 | imu_data.acc=Vector3d(ay,ax,-az); 117 | } 118 | else{ 119 | imu_data.gyr=Vector3d(gx,gy,gz); 120 | imu_data.acc=Vector3d(ax,ay,az); 121 | } 122 | imu_data.acc*=imu->property.rate; 123 | imu_data.gyr*=imu->property.rate; 124 | addImuData(&imu_data,imu); 125 | } 126 | 127 | return true; 128 | } 129 | 130 | static bool readImuDataDefault(std::fstream &file_fp,imu_t *imu) 131 | { 132 | vector data; 133 | imuDataUnit_t imu_data; 134 | while(file_fp.read((char*)data.data(),sizeof(double)*7)){ 135 | imu_data.sow=data[0]; 136 | imu_data.gyr[0]= data[1]; 137 | imu_data.gyr[1]= data[2]; 138 | imu_data.gyr[2]= data[3]; 139 | imu_data.acc[0]=data[4]; 140 | imu_data.acc[1]=data[5]; 141 | imu_data.acc[2]=data[6]; 142 | addImuData(&imu_data,imu); 143 | data.clear(); 144 | } 145 | return imu->n>0; 146 | } 147 | 148 | extern bool loadImu(double ts,double te,string imu_file,imuopt_t imu_opt,imu_t *imu) 149 | { 150 | if(imu_file.empty()){ 151 | LOG(ERROR)<<"imu file error"; 152 | return false; 153 | } 154 | 155 | bool stat=false; 156 | std::fstream file_fp; 157 | file_fp.open(imu_file,std::ios_base::in); 158 | if(!file_fp.is_open()) return false; 159 | 160 | switch (imu->property.fmt) { 161 | case +E_ImuFmt::PSINS: 162 | stat = readImuDataPsins(file_fp,imu_opt,imu); 163 | break; 164 | case +E_ImuFmt::NOVATEL: 165 | stat = readImuDataNvt(ts,te,file_fp,imu_opt,imu); 166 | break; 167 | case +E_ImuFmt::M39: 168 | break; 169 | case +E_ImuFmt::GINS: 170 | stat = readImuDataGins(file_fp,imu_opt,imu); 171 | break; 172 | case +E_ImuFmt::A15: 173 | break; 174 | case +E_ImuFmt::DEFAULT: 175 | stat = readImuDataDefault(file_fp,imu); 176 | break; 177 | } 178 | file_fp.close(); 179 | return stat; 180 | } 181 | 182 | extern bool inputImu(const imu_t *imu,int sample,const epImuData_t &pre_imu,epMeas_t &ep_meas,Vector3d &da,Vector3d &dv,bool back) 183 | { 184 | int sample_=sample; 185 | if(sample==-1) sample_=1; 186 | 187 | ep_meas.imu.data.clear(); 188 | if(back){ 189 | if(ep_meas.idx.imu>imu->n||ep_meas.idx.imu-sample_<0) return false; 190 | for(int i=ep_meas.idx.imu;i>ep_meas.idx.imu-sample_;i--){ 191 | ep_meas.imu.data.push_back(imu->data[i+1]); 192 | ep_meas.imu.data[0].sow=imu->data[i].sow; 193 | } 194 | ep_meas.idx.imu-=sample_; 195 | } 196 | else{ 197 | if(ep_meas.idx.imu<0||ep_meas.idx.imu+sample_>imu->n) return false; 198 | for(int i=ep_meas.idx.imu;idata[i]); 200 | } 201 | ep_meas.idx.imu+=sample_; 202 | } 203 | 204 | ep_meas.imu.sow=ep_meas.imu.data[0].sow; 205 | ep_meas.imu.dt=back?-1.0/imu->property.rate:1.0/imu->property.rate; 206 | ep_meas.imu.da=V30; 207 | ep_meas.imu.dv=V30; 208 | 209 | getIncreMeas(imu->property.rate,back,1,pre_imu,ep_meas.imu); 210 | imuCompensate(pre_imu,ep_meas.imu,da,dv); 211 | 212 | return true; 213 | } 214 | 215 | extern void imuInterpolate(const epImuData_t &imu0,epImuData_t &imu2,double t,epImuData_t &imu1,bool back) 216 | { 217 | if(!back&&(imu0.sow>t||imu2.sowt||imu0.sowsow-pre_imu.data.back().sow; 245 | if(dt>(1.0/rate)*1.5){ 246 | LOG(WARNING)<(1.0/rate)*1.5){ 254 | LOG(WARNING)<ang); 272 | da = cur_imu.data.begin()->ang + dphi; 273 | 274 | scull = 1.0/12.0*(pre_imu.da.cross(cur_imu.data.begin()->vel)+pre_imu.dv.cross(cur_imu.data.begin()->ang)); 275 | rot = 1.0/2.0*(cur_imu.data.begin()->ang.cross(cur_imu.data.begin()->vel)); 276 | 277 | dv = cur_imu.data.begin()->vel + scull+rot; 278 | dv=cur_imu.data.begin()->vel+scull; 279 | cur_imu.da=cur_imu.data.begin()->ang; 280 | cur_imu.dv=cur_imu.data.begin()->vel; 281 | cur_imu.dt=cur_imu.data.begin()->dt; 282 | } 283 | 284 | 285 | 286 | -------------------------------------------------------------------------------- /src/sensors/imu/Imu.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 10/2/22. 3 | // 4 | 5 | #ifndef FUSING_IMU_H 6 | #define FUSING_IMU_H 7 | 8 | #include 9 | #include "EigenInc.h" 10 | #include "Enums.h" 11 | #include "Logger.h" 12 | #include "Config.h" 13 | 14 | using namespace std; 15 | 16 | struct epMeas_t; 17 | 18 | typedef struct { 19 | Matrix3d pos,vel,att; 20 | Matrix3d ba,bg; 21 | }stateCov_t; 22 | 23 | typedef struct{ 24 | double sow; 25 | Vector3d pos,vel,acc,att; 26 | Matrix3d dcm; 27 | Quaterniond quat; 28 | Vector3d ba,bg; 29 | Vector3d sa,sg; 30 | stateCov_t cov; 31 | }state_t; 32 | 33 | typedef struct{ 34 | Vector3d acc_vrw,gyr_arw; 35 | Vector3d gb_std,ab_std; 36 | Vector3d gs_std,as_std; 37 | double tau_bg,tau_ba; 38 | }imuNoise_t; 39 | 40 | typedef struct{ 41 | Vector3d std_pos,std_vel,std_att; 42 | Vector3d std_ba,std_bg; 43 | Vector3d std_sa,std_sg; 44 | }imuError_t; 45 | 46 | struct imuProperty_t{ 47 | E_ImuFmt fmt; 48 | int week; 49 | double rate; 50 | imuNoise_t imu_noise; 51 | imuError_t imu_error; 52 | state_t imu_init; 53 | Vector3d ig_lever; 54 | }; 55 | 56 | typedef struct{ 57 | double sow; 58 | double dt; 59 | Vector3d acc,vel; 60 | Vector3d gyr,ang; 61 | }imuDataUnit_t; 62 | 63 | typedef struct{ 64 | double sow; 65 | double dt; 66 | Vector3d da,dv; /*no correction, eg rot or scull error*/ 67 | vector data; 68 | }epImuData_t; 69 | 70 | typedef struct{ 71 | int n,nmax; 72 | imuDataUnit_t *data; 73 | imuProperty_t property; 74 | }imu_t; 75 | 76 | extern bool loadImu(double ts,double te,string imu_file,imuopt_t imu_opt,imu_t *imu); 77 | extern bool inputImu(const imu_t *imu,int sample,const epImuData_t &pre_imu,epMeas_t &ep_meas,Vector3d &da,Vector3d &dv,bool back); 78 | extern void imuInterpolate(const epImuData_t &imu0,epImuData_t &imu2,double t,epImuData_t &imu1,bool back); 79 | extern void getIncreMeas(double rate,bool back,int epoch,const epImuData_t &pre_imu,epImuData_t &cur_imu); 80 | extern void imuCompensate(const epImuData_t &pre_imu,epImuData_t &cur_imu,Vector3d &da,Vector3d& dv); 81 | 82 | #endif //FUSING_IMU_H 83 | -------------------------------------------------------------------------------- /src/sensors/imu/Ins.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hw on 10/6/22. 3 | // 4 | 5 | #ifndef FUSING_INS_H 6 | #define FUSING_INS_H 7 | 8 | #include "Imu.h" 9 | 10 | struct fusing_t; 11 | struct fusingMeas_t; 12 | 13 | typedef struct{ 14 | Vector3d w_nie,w_nen,w_nin,w_nien; 15 | double tl,sl,cl; 16 | double RNh,RMh,clRNh; 17 | Matrix3d Mpv=Matrix3d::Zero(),DPne=Matrix3d::Zero(); 18 | Vector3d gn,gcc; 19 | }earth_t; 20 | 21 | typedef struct{ 22 | int epoch=0; 23 | state_t state; 24 | Vector3d ig_lever; 25 | double dt; 26 | Vector3d da,dv; 27 | Vector3d fb,wib,fn,web; 28 | earth_t earth; 29 | epImuData_t pre_imu={0}; 30 | int gstat=SOLQ_NONE; 31 | int ns; 32 | }ins_t; 33 | 34 | extern void getStateCov(const MatrixXd& P,state_t &state); 35 | extern int processTimeControl(double ts,double te, double sow, bool back); 36 | extern bool insAlign(const fusingopt_t &opt,const fusingMeas_t &meas,fusing_t &fusing,bool back); 37 | extern bool insUpdate(const imuopt_t &opt,epImuData_t &ep_imu,ins_t &ins, bool back); 38 | 39 | #endif //FUSING_INS_H 40 | --------------------------------------------------------------------------------