├── .gitignore ├── 3rd └── GeographicLib │ ├── CMakeLists.txt │ ├── include │ └── Geocentric │ │ ├── Config.h │ │ ├── Constants.hpp │ │ ├── Geocentric.hpp │ │ ├── LocalCartesian.hpp │ │ └── Math.hpp │ └── src │ ├── Geocentric.cpp │ ├── LocalCartesian.cpp │ └── Math.cpp ├── CMakeLists.txt ├── app └── gps_imu_fusion.cpp ├── cmake ├── YAML.cmake ├── eigen.cmake └── glog.cmake ├── config └── config.yaml ├── data ├── display_path.py ├── images │ ├── 0.png │ ├── 1.png │ ├── 2.png │ └── 3.png └── raw_data │ ├── accel-0.csv │ ├── gps-0.csv │ ├── gps_time.csv │ ├── gyro-0.csv │ ├── ref_accel.csv │ ├── ref_att_quat.csv │ ├── ref_gps.csv │ ├── ref_gyro.csv │ ├── ref_pos.csv │ └── time.csv ├── gnss-inss-sim ├── my_test.csv ├── my_test.py └── readme.md ├── include ├── common_tool.h ├── config_parameters.h ├── eskf.h ├── eskf_flow.h ├── gps_data.h ├── gps_tool.h ├── imu_data.h ├── imu_tool.h └── observability_analysis.h ├── readme.md ├── src ├── eskf.cpp ├── eskf_flow.cpp ├── gps_tool.cpp ├── imu_tool.cpp └── observability_analysis.cpp └── test ├── test_gps.cpp ├── test_imu.cpp └── test_tool.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Created by .ignore support plugin (hsz.mobi) 2 | ### C++ template 3 | # Prerequisites 4 | *.d 5 | 6 | # Compiled Object files 7 | *.slo 8 | *.lo 9 | *.o 10 | *.obj 11 | 12 | # Precompiled Headers 13 | *.gch 14 | *.pch 15 | 16 | # Compiled Dynamic libraries 17 | *.so 18 | *.dylib 19 | *.dll 20 | 21 | # Fortran module files 22 | *.mod 23 | *.smod 24 | 25 | # Compiled Static libraries 26 | *.lai 27 | *.la 28 | *.a 29 | *.lib 30 | 31 | # Executables 32 | *.exe 33 | *.out 34 | *.app 35 | 36 | ### C template 37 | # Prerequisites 38 | *.d 39 | 40 | # Object files 41 | *.o 42 | *.ko 43 | *.obj 44 | *.elf 45 | 46 | # Linker output 47 | *.ilk 48 | *.map 49 | *.exp 50 | 51 | # Precompiled Headers 52 | *.gch 53 | *.pch 54 | 55 | # Libraries 56 | *.lib 57 | *.a 58 | *.la 59 | *.lo 60 | 61 | # Shared objects (inc. Windows DLLs) 62 | *.dll 63 | *.so 64 | *.so.* 65 | *.dylib 66 | 67 | # Executables 68 | *.exe 69 | *.out 70 | *.app 71 | *.i*86 72 | *.x86_64 73 | *.hex 74 | 75 | # Debug files 76 | *.dSYM/ 77 | *.su 78 | *.idb 79 | *.pdb 80 | 81 | # Kernel Module Compile Results 82 | *.mod* 83 | *.cmd 84 | .tmp_versions/ 85 | modules.order 86 | Module.symvers 87 | Mkfile.old 88 | dkms.conf 89 | 90 | # folder 91 | /build 92 | /cmake-build-debug 93 | *.idea/ 94 | -------------------------------------------------------------------------------- /3rd/GeographicLib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(GeographicLib) 2 | 3 | # Version information 4 | set(PROJECT_VERSION_MAJOR 1) 5 | set(PROJECT_VERSION_MINOR 49) 6 | set(PROJECT_VERSION_PATCH 0) 7 | set(PROJECT_VERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}") 8 | if (PROJECT_VERSION_PATCH GREATER 0) 9 | set(PROJECT_VERSION "${PROJECT_VERSION}.${PROJECT_VERSION_PATCH}") 10 | endif () 11 | 12 | # The library version tracks the numbering given by libtool in the 13 | # autoconf set up. 14 | set(LIBVERSION_API 17) 15 | set(LIBVERSION_BUILD 17.1.2) 16 | string(TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) 17 | string(TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER) 18 | 19 | cmake_minimum_required(VERSION 2.8.4) # This version was released 2011-02-16 20 | 21 | # (7) Set the default "real" precision. This should probably be left 22 | # at 2 (double). 23 | set(GEOGRAPHICLIB_PRECISION 2 CACHE STRING 24 | "Precision: 1 = float, 2 = double, 3 = extended, 4 = quadruple, 5 = variable") 25 | set_property(CACHE GEOGRAPHICLIB_PRECISION PROPERTY STRINGS 1 2 3 4 5) 26 | 27 | 28 | set(LIBNAME Geographic) 29 | 30 | include_directories( 31 | ./include/ 32 | ) 33 | 34 | add_library(libGeographiccc SHARED 35 | src/LocalCartesian.cpp 36 | src/Geocentric.cpp 37 | src/Math.cpp) -------------------------------------------------------------------------------- /3rd/GeographicLib/include/Geocentric/Config.h: -------------------------------------------------------------------------------- 1 | // This will be overwritten by ./configure 2 | 3 | #define GEOGRAPHICLIB_VERSION_STRING "1.49" 4 | #define GEOGRAPHICLIB_VERSION_MAJOR 1 5 | #define GEOGRAPHICLIB_VERSION_MINOR 49 6 | #define GEOGRAPHICLIB_VERSION_PATCH 0 7 | 8 | // Undefine HAVE_LONG_DOUBLE if this type is unknown to the compiler 9 | #define GEOGRAPHICLIB_HAVE_LONG_DOUBLE 1 10 | 11 | // Define WORDS_BIGENDIAN to be 1 if your machine is big endian 12 | /* #undef WORDS_BIGENDIAN */ 13 | -------------------------------------------------------------------------------- /3rd/GeographicLib/include/Geocentric/Constants.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file Constants.hpp 3 | * \brief Header for GeographicLib::Constants class 4 | * 5 | * Copyright (c) Charles Karney (2008-2016) and licensed 6 | * under the MIT/X11 License. For more information, see 7 | * https://geographiclib.sourceforge.io/ 8 | **********************************************************************/ 9 | 10 | #if !defined(GEOGRAPHICLIB_CONSTANTS_HPP) 11 | #define GEOGRAPHICLIB_CONSTANTS_HPP 1 12 | 13 | #include "Config.h" 14 | 15 | /** 16 | * @relates GeographicLib::Constants 17 | * Pack the version components into a single integer. Users should not rely on 18 | * this particular packing of the components of the version number; see the 19 | * documentation for GEOGRAPHICLIB_VERSION, below. 20 | **********************************************************************/ 21 | #define GEOGRAPHICLIB_VERSION_NUM(a,b,c) ((((a) * 10000 + (b)) * 100) + (c)) 22 | 23 | /** 24 | * @relates GeographicLib::Constants 25 | * The version of GeographicLib as a single integer, packed as MMmmmmpp where 26 | * MM is the major version, mmmm is the minor version, and pp is the patch 27 | * level. Users should not rely on this particular packing of the components 28 | * of the version number. Instead they should use a test such as \code 29 | #if GEOGRAPHICLIB_VERSION >= GEOGRAPHICLIB_VERSION_NUM(1,37,0) 30 | ... 31 | #endif 32 | * \endcode 33 | **********************************************************************/ 34 | #define GEOGRAPHICLIB_VERSION \ 35 | GEOGRAPHICLIB_VERSION_NUM(GEOGRAPHICLIB_VERSION_MAJOR, \ 36 | GEOGRAPHICLIB_VERSION_MINOR, \ 37 | GEOGRAPHICLIB_VERSION_PATCH) 38 | 39 | /** 40 | * @relates GeographicLib::Constants 41 | * Is the C++11 static_assert available? 42 | **********************************************************************/ 43 | #if !defined(GEOGRAPHICLIB_HAS_STATIC_ASSERT) 44 | # if __cplusplus >= 201103 || defined(__GXX_EXPERIMENTAL_CXX0X__) 45 | # define GEOGRAPHICLIB_HAS_STATIC_ASSERT 1 46 | # elif defined(_MSC_VER) && _MSC_VER >= 1600 47 | // For reference, here is a table of Visual Studio and _MSC_VER 48 | // correspondences: 49 | // 50 | // _MSC_VER Visual Studio 51 | // 1100 vc5 52 | // 1200 vc6 53 | // 1300 vc7 54 | // 1310 vc7.1 (2003) 55 | // 1400 vc8 (2005) 56 | // 1500 vc9 (2008) 57 | // 1600 vc10 (2010) 58 | // 1700 vc11 (2012) 59 | // 1800 vc12 (2013) 60 | // 1900 vc14 (2015) 61 | // 1910+ vc15 (2017) 62 | # define GEOGRAPHICLIB_HAS_STATIC_ASSERT 1 63 | # else 64 | # define GEOGRAPHICLIB_HAS_STATIC_ASSERT 0 65 | # endif 66 | #endif 67 | 68 | /** 69 | * @relates GeographicLib::Constants 70 | * A compile-time assert. Use C++11 static_assert, if available. 71 | **********************************************************************/ 72 | #if !defined(GEOGRAPHICLIB_STATIC_ASSERT) 73 | # if GEOGRAPHICLIB_HAS_STATIC_ASSERT 74 | # define GEOGRAPHICLIB_STATIC_ASSERT static_assert 75 | # else 76 | # define GEOGRAPHICLIB_STATIC_ASSERT(cond,reason) \ 77 | { enum{ GEOGRAPHICLIB_STATIC_ASSERT_ENUM = 1/int(cond) }; } 78 | # endif 79 | #endif 80 | 81 | #if defined(_MSC_VER) && defined(GEOGRAPHICLIB_SHARED_LIB) && \ 82 | GEOGRAPHICLIB_SHARED_LIB 83 | # if GEOGRAPHICLIB_SHARED_LIB > 1 84 | # error GEOGRAPHICLIB_SHARED_LIB must be 0 or 1 85 | # elif defined(GeographicLib_EXPORTS) 86 | # define GEOGRAPHICLIB_EXPORT __declspec(dllexport) 87 | # else 88 | # define GEOGRAPHICLIB_EXPORT __declspec(dllimport) 89 | # endif 90 | #else 91 | # define GEOGRAPHICLIB_EXPORT 92 | #endif 93 | 94 | // Use GEOGRAPHICLIB_DEPRECATED to mark functions, types or variables as 95 | // deprecated. Code inspired by Apache Subversion's svn_types.h file (via 96 | // MPFR). 97 | #if defined(__GNUC__) 98 | # if __GNUC__ > 4 99 | # define GEOGRAPHICLIB_DEPRECATED(msg) __attribute__((deprecated(msg))) 100 | # else 101 | # define GEOGRAPHICLIB_DEPRECATED(msg) __attribute__((deprecated)) 102 | # endif 103 | #elif defined(_MSC_VER) && _MSC_VER >= 1300 104 | # define GEOGRAPHICLIB_DEPRECATED(msg) __declspec(deprecated(msg)) 105 | #else 106 | # define GEOGRAPHICLIB_DEPRECATED(msg) 107 | #endif 108 | 109 | #include 110 | #include 111 | #include "Math.hpp" 112 | 113 | /** 114 | * \brief Namespace for %GeographicLib 115 | * 116 | * All of %GeographicLib is defined within the GeographicLib namespace. In 117 | * addition all the header files are included via %GeographicLib/Class.hpp. 118 | * This minimizes the likelihood of conflicts with other packages. 119 | **********************************************************************/ 120 | namespace GeographicLib { 121 | 122 | /** 123 | * \brief %Constants needed by %GeographicLib 124 | * 125 | * Define constants specifying the WGS84 ellipsoid, the UTM and UPS 126 | * projections, and various unit conversions. 127 | * 128 | * Example of use: 129 | * \include example-Constants.cpp 130 | **********************************************************************/ 131 | class GEOGRAPHICLIB_EXPORT Constants { 132 | private: 133 | typedef Math::real real; 134 | Constants(); // Disable constructor 135 | 136 | public: 137 | /** 138 | * A synonym for Math::degree(). 139 | **********************************************************************/ 140 | static Math::real degree() { return Math::degree(); } 141 | /** 142 | * @return the number of radians in an arcminute. 143 | **********************************************************************/ 144 | static Math::real arcminute() 145 | { return Math::degree() / 60; } 146 | /** 147 | * @return the number of radians in an arcsecond. 148 | **********************************************************************/ 149 | static Math::real arcsecond() 150 | { return Math::degree() / 3600; } 151 | 152 | /** \name Ellipsoid parameters 153 | **********************************************************************/ 154 | ///@{ 155 | /** 156 | * @tparam T the type of the returned value. 157 | * @return the equatorial radius of WGS84 ellipsoid (6378137 m). 158 | **********************************************************************/ 159 | template static T WGS84_a() 160 | { return 6378137 * meter(); } 161 | /** 162 | * A synonym for WGS84_a(). 163 | **********************************************************************/ 164 | static Math::real WGS84_a() { return WGS84_a(); } 165 | /** 166 | * @tparam T the type of the returned value. 167 | * @return the flattening of WGS84 ellipsoid (1/298.257223563). 168 | **********************************************************************/ 169 | template static T WGS84_f() { 170 | // Evaluating this as 1000000000 / T(298257223563LL) reduces the 171 | // round-off error by about 10%. However, expressing the flattening as 172 | // 1/298.257223563 is well ingrained. 173 | return 1 / ( T(298257223563LL) / 1000000000 ); 174 | } 175 | /** 176 | * A synonym for WGS84_f(). 177 | **********************************************************************/ 178 | static Math::real WGS84_f() { return WGS84_f(); } 179 | /** 180 | * @tparam T the type of the returned value. 181 | * @return the gravitational constant of the WGS84 ellipsoid, \e GM, in 182 | * m3 s−2. 183 | **********************************************************************/ 184 | template static T WGS84_GM() 185 | { return T(3986004) * 100000000 + 41800000; } 186 | /** 187 | * A synonym for WGS84_GM(). 188 | **********************************************************************/ 189 | static Math::real WGS84_GM() { return WGS84_GM(); } 190 | /** 191 | * @tparam T the type of the returned value. 192 | * @return the angular velocity of the WGS84 ellipsoid, ω, in rad 193 | * s−1. 194 | **********************************************************************/ 195 | template static T WGS84_omega() 196 | { return 7292115 / (T(1000000) * 100000); } 197 | /** 198 | * A synonym for WGS84_omega(). 199 | **********************************************************************/ 200 | static Math::real WGS84_omega() { return WGS84_omega(); } 201 | /** 202 | * @tparam T the type of the returned value. 203 | * @return the equatorial radius of GRS80 ellipsoid, \e a, in m. 204 | **********************************************************************/ 205 | template static T GRS80_a() 206 | { return 6378137 * meter(); } 207 | /** 208 | * A synonym for GRS80_a(). 209 | **********************************************************************/ 210 | static Math::real GRS80_a() { return GRS80_a(); } 211 | /** 212 | * @tparam T the type of the returned value. 213 | * @return the gravitational constant of the GRS80 ellipsoid, \e GM, in 214 | * m3 s−2. 215 | **********************************************************************/ 216 | template static T GRS80_GM() 217 | { return T(3986005) * 100000000; } 218 | /** 219 | * A synonym for GRS80_GM(). 220 | **********************************************************************/ 221 | static Math::real GRS80_GM() { return GRS80_GM(); } 222 | /** 223 | * @tparam T the type of the returned value. 224 | * @return the angular velocity of the GRS80 ellipsoid, ω, in rad 225 | * s−1. 226 | * 227 | * This is about 2 π 366.25 / (365.25 × 24 × 3600) rad 228 | * s−1. 365.25 is the number of days in a Julian year and 229 | * 365.35/366.25 converts from solar days to sidereal days. Using the 230 | * number of days in a Gregorian year (365.2425) results in a worse 231 | * approximation (because the Gregorian year includes the precession of the 232 | * earth's axis). 233 | **********************************************************************/ 234 | template static T GRS80_omega() 235 | { return 7292115 / (T(1000000) * 100000); } 236 | /** 237 | * A synonym for GRS80_omega(). 238 | **********************************************************************/ 239 | static Math::real GRS80_omega() { return GRS80_omega(); } 240 | /** 241 | * @tparam T the type of the returned value. 242 | * @return the dynamical form factor of the GRS80 ellipsoid, 243 | * J2. 244 | **********************************************************************/ 245 | template static T GRS80_J2() 246 | { return T(108263) / 100000000; } 247 | /** 248 | * A synonym for GRS80_J2(). 249 | **********************************************************************/ 250 | static Math::real GRS80_J2() { return GRS80_J2(); } 251 | /** 252 | * @tparam T the type of the returned value. 253 | * @return the central scale factor for UTM (0.9996). 254 | **********************************************************************/ 255 | template static T UTM_k0() 256 | {return T(9996) / 10000; } 257 | /** 258 | * A synonym for UTM_k0(). 259 | **********************************************************************/ 260 | static Math::real UTM_k0() { return UTM_k0(); } 261 | /** 262 | * @tparam T the type of the returned value. 263 | * @return the central scale factor for UPS (0.994). 264 | **********************************************************************/ 265 | template static T UPS_k0() 266 | { return T(994) / 1000; } 267 | /** 268 | * A synonym for UPS_k0(). 269 | **********************************************************************/ 270 | static Math::real UPS_k0() { return UPS_k0(); } 271 | ///@} 272 | 273 | /** \name SI units 274 | **********************************************************************/ 275 | ///@{ 276 | /** 277 | * @tparam T the type of the returned value. 278 | * @return the number of meters in a meter. 279 | * 280 | * This is unity, but this lets the internal system of units be changed if 281 | * necessary. 282 | **********************************************************************/ 283 | template static T meter() { return T(1); } 284 | /** 285 | * A synonym for meter(). 286 | **********************************************************************/ 287 | static Math::real meter() { return meter(); } 288 | /** 289 | * @return the number of meters in a kilometer. 290 | **********************************************************************/ 291 | static Math::real kilometer() 292 | { return 1000 * meter(); } 293 | /** 294 | * @return the number of meters in a nautical mile (approximately 1 arc 295 | * minute) 296 | **********************************************************************/ 297 | static Math::real nauticalmile() 298 | { return 1852 * meter(); } 299 | 300 | /** 301 | * @tparam T the type of the returned value. 302 | * @return the number of square meters in a square meter. 303 | * 304 | * This is unity, but this lets the internal system of units be changed if 305 | * necessary. 306 | **********************************************************************/ 307 | template static T square_meter() 308 | { return meter() * meter(); } 309 | /** 310 | * A synonym for square_meter(). 311 | **********************************************************************/ 312 | static Math::real square_meter() 313 | { return square_meter(); } 314 | /** 315 | * @return the number of square meters in a hectare. 316 | **********************************************************************/ 317 | static Math::real hectare() 318 | { return 10000 * square_meter(); } 319 | /** 320 | * @return the number of square meters in a square kilometer. 321 | **********************************************************************/ 322 | static Math::real square_kilometer() 323 | { return kilometer() * kilometer(); } 324 | /** 325 | * @return the number of square meters in a square nautical mile. 326 | **********************************************************************/ 327 | static Math::real square_nauticalmile() 328 | { return nauticalmile() * nauticalmile(); } 329 | ///@} 330 | 331 | /** \name Anachronistic British units 332 | **********************************************************************/ 333 | ///@{ 334 | /** 335 | * @return the number of meters in an international foot. 336 | **********************************************************************/ 337 | static Math::real foot() 338 | { return real(254 * 12) / 10000 * meter(); } 339 | /** 340 | * @return the number of meters in a yard. 341 | **********************************************************************/ 342 | static Math::real yard() { return 3 * foot(); } 343 | /** 344 | * @return the number of meters in a fathom. 345 | **********************************************************************/ 346 | static Math::real fathom() { return 2 * yard(); } 347 | /** 348 | * @return the number of meters in a chain. 349 | **********************************************************************/ 350 | static Math::real chain() { return 22 * yard(); } 351 | /** 352 | * @return the number of meters in a furlong. 353 | **********************************************************************/ 354 | static Math::real furlong() { return 10 * chain(); } 355 | /** 356 | * @return the number of meters in a statute mile. 357 | **********************************************************************/ 358 | static Math::real mile() { return 8 * furlong(); } 359 | /** 360 | * @return the number of square meters in an acre. 361 | **********************************************************************/ 362 | static Math::real acre() { return chain() * furlong(); } 363 | /** 364 | * @return the number of square meters in a square statute mile. 365 | **********************************************************************/ 366 | static Math::real square_mile() { return mile() * mile(); } 367 | ///@} 368 | 369 | /** \name Anachronistic US units 370 | **********************************************************************/ 371 | ///@{ 372 | /** 373 | * @return the number of meters in a US survey foot. 374 | **********************************************************************/ 375 | static Math::real surveyfoot() 376 | { return real(1200) / 3937 * meter(); } 377 | ///@} 378 | }; 379 | 380 | /** 381 | * \brief Exception handling for %GeographicLib 382 | * 383 | * A class to handle exceptions. It's derived from std::runtime_error so it 384 | * can be caught by the usual catch clauses. 385 | * 386 | * Example of use: 387 | * \include example-GeographicErr.cpp 388 | **********************************************************************/ 389 | class GeographicErr : public std::runtime_error { 390 | public: 391 | 392 | /** 393 | * Constructor 394 | * 395 | * @param[in] msg a string message, which is accessible in the catch 396 | * clause via what(). 397 | **********************************************************************/ 398 | GeographicErr(const std::string& msg) : std::runtime_error(msg) {} 399 | }; 400 | 401 | } // namespace GeographicLib 402 | 403 | #endif // GEOGRAPHICLIB_CONSTANTS_HPP 404 | -------------------------------------------------------------------------------- /3rd/GeographicLib/include/Geocentric/Geocentric.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file Geocentric.hpp 3 | * \brief Header for GeographicLib::Geocentric class 4 | * 5 | * Copyright (c) Charles Karney (2008-2016) and licensed 6 | * under the MIT/X11 License. For more information, see 7 | * https://geographiclib.sourceforge.io/ 8 | **********************************************************************/ 9 | 10 | #if !defined(GEOGRAPHICLIB_GEOCENTRIC_HPP) 11 | #define GEOGRAPHICLIB_GEOCENTRIC_HPP 1 12 | 13 | #include 14 | #include "Constants.hpp" 15 | 16 | namespace GeographicLib { 17 | 18 | /** 19 | * \brief %Geocentric coordinates 20 | * 21 | * Convert between geodetic coordinates latitude = \e lat, longitude = \e 22 | * lon, height = \e h (measured vertically from the surface of the ellipsoid) 23 | * to geocentric coordinates (\e X, \e Y, \e Z). The origin of geocentric 24 | * coordinates is at the center of the earth. The \e Z axis goes thru the 25 | * north pole, \e lat = 90°. The \e X axis goes thru \e lat = 0, 26 | * \e lon = 0. %Geocentric coordinates are also known as earth centered, 27 | * earth fixed (ECEF) coordinates. 28 | * 29 | * The conversion from geographic to geocentric coordinates is 30 | * straightforward. For the reverse transformation we use 31 | * - H. Vermeille, 32 | * Direct 33 | * transformation from geocentric coordinates to geodetic coordinates, 34 | * J. Geodesy 76, 451--454 (2002). 35 | * . 36 | * Several changes have been made to ensure that the method returns accurate 37 | * results for all finite inputs (even if \e h is infinite). The changes are 38 | * described in Appendix B of 39 | * - C. F. F. Karney, 40 | * Geodesics 41 | * on an ellipsoid of revolution, 42 | * Feb. 2011; 43 | * preprint 44 | * arxiv:1102.1215v1. 45 | * . 46 | * Vermeille similarly updated his method in 47 | * - H. Vermeille, 48 | * 49 | * An analytical method to transform geocentric into 50 | * geodetic coordinates, J. Geodesy 85, 105--117 (2011). 51 | * . 52 | * See \ref geocentric for more information. 53 | * 54 | * The errors in these routines are close to round-off. Specifically, for 55 | * points within 5000 km of the surface of the ellipsoid (either inside or 56 | * outside the ellipsoid), the error is bounded by 7 nm (7 nanometers) for 57 | * the WGS84 ellipsoid. See \ref geocentric for further information on the 58 | * errors. 59 | * 60 | * Example of use: 61 | * \include example-Geocentric.cpp 62 | * 63 | * CartConvert is a command-line utility 64 | * providing access to the functionality of Geocentric and LocalCartesian. 65 | **********************************************************************/ 66 | 67 | class GEOGRAPHICLIB_EXPORT Geocentric { 68 | private: 69 | typedef Math::real real; 70 | friend class LocalCartesian; 71 | friend class MagneticCircle; // MagneticCircle uses Rotation 72 | friend class MagneticModel; // MagneticModel uses IntForward 73 | friend class GravityCircle; // GravityCircle uses Rotation 74 | friend class GravityModel; // GravityModel uses IntForward 75 | friend class NormalGravity; // NormalGravity uses IntForward 76 | static const size_t dim_ = 3; 77 | static const size_t dim2_ = dim_ * dim_; 78 | real _a, _f, _e2, _e2m, _e2a, _e4a, _maxrad; 79 | static void Rotation(real sphi, real cphi, real slam, real clam, 80 | real M[dim2_]); 81 | static void Rotate(real M[dim2_], real x, real y, real z, 82 | real& X, real& Y, real& Z) { 83 | // Perform [X,Y,Z]^t = M.[x,y,z]^t 84 | // (typically local cartesian to geocentric) 85 | X = M[0] * x + M[1] * y + M[2] * z; 86 | Y = M[3] * x + M[4] * y + M[5] * z; 87 | Z = M[6] * x + M[7] * y + M[8] * z; 88 | } 89 | static void Unrotate(real M[dim2_], real X, real Y, real Z, 90 | real& x, real& y, real& z) { 91 | // Perform [x,y,z]^t = M^t.[X,Y,Z]^t 92 | // (typically geocentric to local cartesian) 93 | x = M[0] * X + M[3] * Y + M[6] * Z; 94 | y = M[1] * X + M[4] * Y + M[7] * Z; 95 | z = M[2] * X + M[5] * Y + M[8] * Z; 96 | } 97 | void IntForward(real lat, real lon, real h, real& X, real& Y, real& Z, 98 | real M[dim2_]) const; 99 | void IntReverse(real X, real Y, real Z, real& lat, real& lon, real& h, 100 | real M[dim2_]) const; 101 | 102 | public: 103 | 104 | /** 105 | * Constructor for a ellipsoid with 106 | * 107 | * @param[in] a equatorial radius (meters). 108 | * @param[in] f flattening of ellipsoid. Setting \e f = 0 gives a sphere. 109 | * Negative \e f gives a prolate ellipsoid. 110 | * @exception GeographicErr if \e a or (1 − \e f) \e a is not 111 | * positive. 112 | **********************************************************************/ 113 | Geocentric(real a, real f); 114 | 115 | /** 116 | * A default constructor (for use by NormalGravity). 117 | **********************************************************************/ 118 | Geocentric() : _a(-1) {} 119 | 120 | /** 121 | * Convert from geodetic to geocentric coordinates. 122 | * 123 | * @param[in] lat latitude of point (degrees). 124 | * @param[in] lon longitude of point (degrees). 125 | * @param[in] h height of point above the ellipsoid (meters). 126 | * @param[out] X geocentric coordinate (meters). 127 | * @param[out] Y geocentric coordinate (meters). 128 | * @param[out] Z geocentric coordinate (meters). 129 | * 130 | * \e lat should be in the range [−90°, 90°]. 131 | **********************************************************************/ 132 | void Forward(real lat, real lon, real h, real& X, real& Y, real& Z) 133 | const { 134 | if (Init()) 135 | IntForward(lat, lon, h, X, Y, Z, NULL); 136 | } 137 | 138 | /** 139 | * Convert from geodetic to geocentric coordinates and return rotation 140 | * matrix. 141 | * 142 | * @param[in] lat latitude of point (degrees). 143 | * @param[in] lon longitude of point (degrees). 144 | * @param[in] h height of point above the ellipsoid (meters). 145 | * @param[out] X geocentric coordinate (meters). 146 | * @param[out] Y geocentric coordinate (meters). 147 | * @param[out] Z geocentric coordinate (meters). 148 | * @param[out] M if the length of the vector is 9, fill with the rotation 149 | * matrix in row-major order. 150 | * 151 | * Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can 152 | * express \e v as \e column vectors in one of two ways 153 | * - in east, north, up coordinates (where the components are relative to a 154 | * local coordinate system at (\e lat, \e lon, \e h)); call this 155 | * representation \e v1. 156 | * - in geocentric \e X, \e Y, \e Z coordinates; call this representation 157 | * \e v0. 158 | * . 159 | * Then we have \e v0 = \e M ⋅ \e v1. 160 | **********************************************************************/ 161 | void Forward(real lat, real lon, real h, real& X, real& Y, real& Z, 162 | std::vector& M) 163 | const { 164 | if (!Init()) 165 | return; 166 | if (M.end() == M.begin() + dim2_) { 167 | real t[dim2_]; 168 | IntForward(lat, lon, h, X, Y, Z, t); 169 | std::copy(t, t + dim2_, M.begin()); 170 | } else 171 | IntForward(lat, lon, h, X, Y, Z, NULL); 172 | } 173 | 174 | /** 175 | * Convert from geocentric to geodetic to coordinates. 176 | * 177 | * @param[in] X geocentric coordinate (meters). 178 | * @param[in] Y geocentric coordinate (meters). 179 | * @param[in] Z geocentric coordinate (meters). 180 | * @param[out] lat latitude of point (degrees). 181 | * @param[out] lon longitude of point (degrees). 182 | * @param[out] h height of point above the ellipsoid (meters). 183 | * 184 | * In general there are multiple solutions and the result which maximizes 185 | * \e h is returned. If there are still multiple solutions with different 186 | * latitudes (applies only if \e Z = 0), then the solution with \e lat > 0 187 | * is returned. If there are still multiple solutions with different 188 | * longitudes (applies only if \e X = \e Y = 0) then \e lon = 0 is 189 | * returned. The value of \e h returned satisfies \e h ≥ − \e a 190 | * (1 − e2) / sqrt(1 − e2 191 | * sin2\e lat). The value of \e lon returned is in the range 192 | * [−180°, 180°]. 193 | **********************************************************************/ 194 | void Reverse(real X, real Y, real Z, real& lat, real& lon, real& h) 195 | const { 196 | if (Init()) 197 | IntReverse(X, Y, Z, lat, lon, h, NULL); 198 | } 199 | 200 | /** 201 | * Convert from geocentric to geodetic to coordinates. 202 | * 203 | * @param[in] X geocentric coordinate (meters). 204 | * @param[in] Y geocentric coordinate (meters). 205 | * @param[in] Z geocentric coordinate (meters). 206 | * @param[out] lat latitude of point (degrees). 207 | * @param[out] lon longitude of point (degrees). 208 | * @param[out] h height of point above the ellipsoid (meters). 209 | * @param[out] M if the length of the vector is 9, fill with the rotation 210 | * matrix in row-major order. 211 | * 212 | * Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can 213 | * express \e v as \e column vectors in one of two ways 214 | * - in east, north, up coordinates (where the components are relative to a 215 | * local coordinate system at (\e lat, \e lon, \e h)); call this 216 | * representation \e v1. 217 | * - in geocentric \e X, \e Y, \e Z coordinates; call this representation 218 | * \e v0. 219 | * . 220 | * Then we have \e v1 = MT ⋅ \e v0, where 221 | * MT is the transpose of \e M. 222 | **********************************************************************/ 223 | void Reverse(real X, real Y, real Z, real& lat, real& lon, real& h, 224 | std::vector& M) 225 | const { 226 | if (!Init()) 227 | return; 228 | if (M.end() == M.begin() + dim2_) { 229 | real t[dim2_]; 230 | IntReverse(X, Y, Z, lat, lon, h, t); 231 | std::copy(t, t + dim2_, M.begin()); 232 | } else 233 | IntReverse(X, Y, Z, lat, lon, h, NULL); 234 | } 235 | 236 | /** \name Inspector functions 237 | **********************************************************************/ 238 | ///@{ 239 | /** 240 | * @return true if the object has been initialized. 241 | **********************************************************************/ 242 | bool Init() const { return _a > 0; } 243 | /** 244 | * @return \e a the equatorial radius of the ellipsoid (meters). This is 245 | * the value used in the constructor. 246 | **********************************************************************/ 247 | Math::real MajorRadius() const 248 | { return Init() ? _a : Math::NaN(); } 249 | 250 | /** 251 | * @return \e f the flattening of the ellipsoid. This is the 252 | * value used in the constructor. 253 | **********************************************************************/ 254 | Math::real Flattening() const 255 | { return Init() ? _f : Math::NaN(); } 256 | ///@} 257 | 258 | /** 259 | * A global instantiation of Geocentric with the parameters for the WGS84 260 | * ellipsoid. 261 | **********************************************************************/ 262 | static const Geocentric& WGS84(); 263 | }; 264 | 265 | } // namespace GeographicLib 266 | 267 | #endif // GEOGRAPHICLIB_GEOCENTRIC_HPP 268 | -------------------------------------------------------------------------------- /3rd/GeographicLib/include/Geocentric/LocalCartesian.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file LocalCartesian.hpp 3 | * \brief Header for GeographicLib::LocalCartesian class 4 | * 5 | * Copyright (c) Charles Karney (2008-2016) and licensed 6 | * under the MIT/X11 License. For more information, see 7 | * https://geographiclib.sourceforge.io/ 8 | **********************************************************************/ 9 | 10 | #if !defined(GEOGRAPHICLIB_LOCALCARTESIAN_HPP) 11 | #define GEOGRAPHICLIB_LOCALCARTESIAN_HPP 1 12 | 13 | #include "Geocentric.hpp" 14 | #include "Constants.hpp" 15 | 16 | namespace GeographicLib { 17 | 18 | /** 19 | * \brief Local cartesian coordinates 20 | * 21 | * Convert between geodetic coordinates latitude = \e lat, longitude = \e 22 | * lon, height = \e h (measured vertically from the surface of the ellipsoid) 23 | * to local cartesian coordinates (\e x, \e y, \e z). The origin of local 24 | * cartesian coordinate system is at \e lat = \e lat0, \e lon = \e lon0, \e h 25 | * = \e h0. The \e z axis is normal to the ellipsoid; the \e y axis points 26 | * due north. The plane \e z = - \e h0 is tangent to the ellipsoid. 27 | * 28 | * The conversions all take place via geocentric coordinates using a 29 | * Geocentric object (by default Geocentric::WGS84()). 30 | * 31 | * Example of use: 32 | * \include example-LocalCartesian.cpp 33 | * 34 | * CartConvert is a command-line utility 35 | * providing access to the functionality of Geocentric and LocalCartesian. 36 | **********************************************************************/ 37 | 38 | class GEOGRAPHICLIB_EXPORT LocalCartesian { 39 | private: 40 | typedef Math::real real; 41 | static const size_t dim_ = 3; 42 | static const size_t dim2_ = dim_ * dim_; 43 | Geocentric _earth; 44 | real _lat0, _lon0, _h0; 45 | real _x0, _y0, _z0, _r[dim2_]; 46 | void IntForward(real lat, real lon, real h, real& x, real& y, real& z, 47 | real M[dim2_]) const; 48 | void IntReverse(real x, real y, real z, real& lat, real& lon, real& h, 49 | real M[dim2_]) const; 50 | void MatrixMultiply(real M[dim2_]) const; 51 | public: 52 | 53 | /** 54 | * Constructor setting the origin. 55 | * 56 | * @param[in] lat0 latitude at origin (degrees). 57 | * @param[in] lon0 longitude at origin (degrees). 58 | * @param[in] h0 height above ellipsoid at origin (meters); default 0. 59 | * @param[in] earth Geocentric object for the transformation; default 60 | * Geocentric::WGS84(). 61 | * 62 | * \e lat0 should be in the range [−90°, 90°]. 63 | **********************************************************************/ 64 | LocalCartesian(real lat0, real lon0, real h0 = 0, 65 | const Geocentric& earth = Geocentric::WGS84()) 66 | : _earth(earth) 67 | { Reset(lat0, lon0, h0); } 68 | 69 | /** 70 | * Default constructor. 71 | * 72 | * @param[in] earth Geocentric object for the transformation; default 73 | * Geocentric::WGS84(). 74 | * 75 | * Sets \e lat0 = 0, \e lon0 = 0, \e h0 = 0. 76 | **********************************************************************/ 77 | explicit LocalCartesian(const Geocentric& earth = Geocentric::WGS84()) 78 | : _earth(earth) 79 | { Reset(real(0), real(0), real(0)); } 80 | 81 | /** 82 | * Reset the origin. 83 | * 84 | * @param[in] lat0 latitude at origin (degrees). 85 | * @param[in] lon0 longitude at origin (degrees). 86 | * @param[in] h0 height above ellipsoid at origin (meters); default 0. 87 | * 88 | * \e lat0 should be in the range [−90°, 90°]. 89 | **********************************************************************/ 90 | void Reset(real lat0, real lon0, real h0 = 0); 91 | 92 | /** 93 | * Convert from geodetic to local cartesian coordinates. 94 | * 95 | * @param[in] lat latitude of point (degrees). 96 | * @param[in] lon longitude of point (degrees). 97 | * @param[in] h height of point above the ellipsoid (meters). 98 | * @param[out] x local cartesian coordinate (meters). 99 | * @param[out] y local cartesian coordinate (meters). 100 | * @param[out] z local cartesian coordinate (meters). 101 | * 102 | * \e lat should be in the range [−90°, 90°]. 103 | **********************************************************************/ 104 | void Forward(real lat, real lon, real h, real& x, real& y, real& z) 105 | const { 106 | IntForward(lat, lon, h, x, y, z, NULL); 107 | } 108 | 109 | /** 110 | * Convert from geodetic to local cartesian coordinates and return rotation 111 | * matrix. 112 | * 113 | * @param[in] lat latitude of point (degrees). 114 | * @param[in] lon longitude of point (degrees). 115 | * @param[in] h height of point above the ellipsoid (meters). 116 | * @param[out] x local cartesian coordinate (meters). 117 | * @param[out] y local cartesian coordinate (meters). 118 | * @param[out] z local cartesian coordinate (meters). 119 | * @param[out] M if the length of the vector is 9, fill with the rotation 120 | * matrix in row-major order. 121 | * 122 | * \e lat should be in the range [−90°, 90°]. 123 | * 124 | * Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can 125 | * express \e v as \e column vectors in one of two ways 126 | * - in east, north, up coordinates (where the components are relative to a 127 | * local coordinate system at (\e lat, \e lon, \e h)); call this 128 | * representation \e v1. 129 | * - in \e x, \e y, \e z coordinates (where the components are relative to 130 | * the local coordinate system at (\e lat0, \e lon0, \e h0)); call this 131 | * representation \e v0. 132 | * . 133 | * Then we have \e v0 = \e M ⋅ \e v1. 134 | **********************************************************************/ 135 | void Forward(real lat, real lon, real h, real& x, real& y, real& z, 136 | std::vector& M) 137 | const { 138 | if (M.end() == M.begin() + dim2_) { 139 | real t[dim2_]; 140 | IntForward(lat, lon, h, x, y, z, t); 141 | std::copy(t, t + dim2_, M.begin()); 142 | } else 143 | IntForward(lat, lon, h, x, y, z, NULL); 144 | } 145 | 146 | /** 147 | * Convert from local cartesian to geodetic coordinates. 148 | * 149 | * @param[in] x local cartesian coordinate (meters). 150 | * @param[in] y local cartesian coordinate (meters). 151 | * @param[in] z local cartesian coordinate (meters). 152 | * @param[out] lat latitude of point (degrees). 153 | * @param[out] lon longitude of point (degrees). 154 | * @param[out] h height of point above the ellipsoid (meters). 155 | * 156 | * The value of \e lon returned is in the range [−180°, 157 | * 180°]. 158 | **********************************************************************/ 159 | void Reverse(real x, real y, real z, real& lat, real& lon, real& h) 160 | const { 161 | IntReverse(x, y, z, lat, lon, h, NULL); 162 | } 163 | 164 | /** 165 | * Convert from local cartesian to geodetic coordinates and return rotation 166 | * matrix. 167 | * 168 | * @param[in] x local cartesian coordinate (meters). 169 | * @param[in] y local cartesian coordinate (meters). 170 | * @param[in] z local cartesian coordinate (meters). 171 | * @param[out] lat latitude of point (degrees). 172 | * @param[out] lon longitude of point (degrees). 173 | * @param[out] h height of point above the ellipsoid (meters). 174 | * @param[out] M if the length of the vector is 9, fill with the rotation 175 | * matrix in row-major order. 176 | * 177 | * Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can 178 | * express \e v as \e column vectors in one of two ways 179 | * - in east, north, up coordinates (where the components are relative to a 180 | * local coordinate system at (\e lat, \e lon, \e h)); call this 181 | * representation \e v1. 182 | * - in \e x, \e y, \e z coordinates (where the components are relative to 183 | * the local coordinate system at (\e lat0, \e lon0, \e h0)); call this 184 | * representation \e v0. 185 | * . 186 | * Then we have \e v1 = MT ⋅ \e v0, where 187 | * MT is the transpose of \e M. 188 | **********************************************************************/ 189 | void Reverse(real x, real y, real z, real& lat, real& lon, real& h, 190 | std::vector& M) 191 | const { 192 | if (M.end() == M.begin() + dim2_) { 193 | real t[dim2_]; 194 | IntReverse(x, y, z, lat, lon, h, t); 195 | std::copy(t, t + dim2_, M.begin()); 196 | } else 197 | IntReverse(x, y, z, lat, lon, h, NULL); 198 | } 199 | 200 | /** \name Inspector functions 201 | **********************************************************************/ 202 | ///@{ 203 | /** 204 | * @return latitude of the origin (degrees). 205 | **********************************************************************/ 206 | Math::real LatitudeOrigin() const { return _lat0; } 207 | 208 | /** 209 | * @return longitude of the origin (degrees). 210 | **********************************************************************/ 211 | Math::real LongitudeOrigin() const { return _lon0; } 212 | 213 | /** 214 | * @return height of the origin (meters). 215 | **********************************************************************/ 216 | Math::real HeightOrigin() const { return _h0; } 217 | 218 | /** 219 | * @return \e a the equatorial radius of the ellipsoid (meters). This is 220 | * the value of \e a inherited from the Geocentric object used in the 221 | * constructor. 222 | **********************************************************************/ 223 | Math::real MajorRadius() const { return _earth.MajorRadius(); } 224 | 225 | /** 226 | * @return \e f the flattening of the ellipsoid. This is the value 227 | * inherited from the Geocentric object used in the constructor. 228 | **********************************************************************/ 229 | Math::real Flattening() const { return _earth.Flattening(); } 230 | ///@} 231 | 232 | }; 233 | 234 | } // namespace GeographicLib 235 | 236 | #endif // GEOGRAPHICLIB_LOCALCARTESIAN_HPP 237 | -------------------------------------------------------------------------------- /3rd/GeographicLib/include/Geocentric/Math.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file Math.hpp 3 | * \brief Header for GeographicLib::Math class 4 | * 5 | * Copyright (c) Charles Karney (2008-2017) and licensed 6 | * under the MIT/X11 License. For more information, see 7 | * https://geographiclib.sourceforge.io/ 8 | **********************************************************************/ 9 | 10 | // Constants.hpp includes Math.hpp. Place this include outside Math.hpp's 11 | // include guard to enforce this ordering. 12 | #include "Constants.hpp" 13 | 14 | #if !defined(GEOGRAPHICLIB_MATH_HPP) 15 | #define GEOGRAPHICLIB_MATH_HPP 1 16 | 17 | /** 18 | * Are C++11 math functions available? 19 | **********************************************************************/ 20 | #if !defined(GEOGRAPHICLIB_CXX11_MATH) 21 | // Recent versions of g++ -std=c++11 (4.7 and later?) set __cplusplus to 201103 22 | // and support the new C++11 mathematical functions, std::atanh, etc. However 23 | // the Android toolchain, which uses g++ -std=c++11 (4.8 as of 2014-03-11, 24 | // according to Pullan Lu), does not support std::atanh. Android toolchains 25 | // might define __ANDROID__ or ANDROID; so need to check both. With OSX the 26 | // version is GNUC version 4.2 and __cplusplus is set to 201103, so remove the 27 | // version check on GNUC. 28 | # if defined(__GNUC__) && __cplusplus >= 201103 && \ 29 | !(defined(__ANDROID__) || defined(ANDROID) || defined(__CYGWIN__)) 30 | # define GEOGRAPHICLIB_CXX11_MATH 1 31 | // Visual C++ 12 supports these functions 32 | # elif defined(_MSC_VER) && _MSC_VER >= 1800 33 | # define GEOGRAPHICLIB_CXX11_MATH 1 34 | # else 35 | # define GEOGRAPHICLIB_CXX11_MATH 0 36 | # endif 37 | #endif 38 | 39 | #if !defined(GEOGRAPHICLIB_WORDS_BIGENDIAN) 40 | # define GEOGRAPHICLIB_WORDS_BIGENDIAN 0 41 | #endif 42 | 43 | #if !defined(GEOGRAPHICLIB_HAVE_LONG_DOUBLE) 44 | # define GEOGRAPHICLIB_HAVE_LONG_DOUBLE 0 45 | #endif 46 | 47 | #if !defined(GEOGRAPHICLIB_PRECISION) 48 | /** 49 | * The precision of floating point numbers used in %GeographicLib. 1 means 50 | * float (single precision); 2 (the default) means double; 3 means long double; 51 | * 4 is reserved for quadruple precision. Nearly all the testing has been 52 | * carried out with doubles and that's the recommended configuration. In order 53 | * for long double to be used, GEOGRAPHICLIB_HAVE_LONG_DOUBLE needs to be 54 | * defined. Note that with Microsoft Visual Studio, long double is the same as 55 | * double. 56 | **********************************************************************/ 57 | # define GEOGRAPHICLIB_PRECISION 2 58 | #endif 59 | 60 | #include 61 | #include 62 | #include 63 | 64 | #if GEOGRAPHICLIB_PRECISION == 4 65 | #include 66 | #if BOOST_VERSION >= 105600 67 | #include 68 | #endif 69 | #include 70 | #include 71 | __float128 fmaq(__float128, __float128, __float128); 72 | #elif GEOGRAPHICLIB_PRECISION == 5 73 | #include 74 | #endif 75 | 76 | #if GEOGRAPHICLIB_PRECISION > 3 77 | // volatile keyword makes no sense for multiprec types 78 | #define GEOGRAPHICLIB_VOLATILE 79 | // Signal a convergence failure with multiprec types by throwing an exception 80 | // at loop exit. 81 | #define GEOGRAPHICLIB_PANIC \ 82 | (throw GeographicLib::GeographicErr("Convergence failure"), false) 83 | #else 84 | #define GEOGRAPHICLIB_VOLATILE volatile 85 | // Ignore convergence failures with standard floating points types by allowing 86 | // loop to exit cleanly. 87 | #define GEOGRAPHICLIB_PANIC false 88 | #endif 89 | 90 | namespace GeographicLib { 91 | 92 | /** 93 | * \brief Mathematical functions needed by %GeographicLib 94 | * 95 | * Define mathematical functions in order to localize system dependencies and 96 | * to provide generic versions of the functions. In addition define a real 97 | * type to be used by %GeographicLib. 98 | * 99 | * Example of use: 100 | * \include example-Math.cpp 101 | **********************************************************************/ 102 | class GEOGRAPHICLIB_EXPORT Math { 103 | private: 104 | void dummy() { 105 | GEOGRAPHICLIB_STATIC_ASSERT(GEOGRAPHICLIB_PRECISION >= 1 && 106 | GEOGRAPHICLIB_PRECISION <= 5, 107 | "Bad value of precision"); 108 | } 109 | Math(); // Disable constructor 110 | public: 111 | 112 | #if GEOGRAPHICLIB_HAVE_LONG_DOUBLE 113 | /** 114 | * The extended precision type for real numbers, used for some testing. 115 | * This is long double on computers with this type; otherwise it is double. 116 | **********************************************************************/ 117 | typedef long double extended; 118 | #else 119 | typedef double extended; 120 | #endif 121 | 122 | #if GEOGRAPHICLIB_PRECISION == 2 123 | /** 124 | * The real type for %GeographicLib. Nearly all the testing has been done 125 | * with \e real = double. However, the algorithms should also work with 126 | * float and long double (where available). (CAUTION: reasonable 127 | * accuracy typically cannot be obtained using floats.) 128 | **********************************************************************/ 129 | typedef double real; 130 | #elif GEOGRAPHICLIB_PRECISION == 1 131 | typedef float real; 132 | #elif GEOGRAPHICLIB_PRECISION == 3 133 | typedef extended real; 134 | #elif GEOGRAPHICLIB_PRECISION == 4 135 | typedef boost::multiprecision::float128 real; 136 | #elif GEOGRAPHICLIB_PRECISION == 5 137 | typedef mpfr::mpreal real; 138 | #else 139 | typedef double real; 140 | #endif 141 | 142 | /** 143 | * @return the number of bits of precision in a real number. 144 | **********************************************************************/ 145 | static int digits() { 146 | #if GEOGRAPHICLIB_PRECISION != 5 147 | return std::numeric_limits::digits; 148 | #else 149 | return std::numeric_limits::digits(); 150 | #endif 151 | } 152 | 153 | /** 154 | * Set the binary precision of a real number. 155 | * 156 | * @param[in] ndigits the number of bits of precision. 157 | * @return the resulting number of bits of precision. 158 | * 159 | * This only has an effect when GEOGRAPHICLIB_PRECISION = 5. See also 160 | * Utility::set_digits for caveats about when this routine should be 161 | * called. 162 | **********************************************************************/ 163 | static int set_digits(int ndigits) { 164 | #if GEOGRAPHICLIB_PRECISION != 5 165 | (void)ndigits; 166 | #else 167 | mpfr::mpreal::set_default_prec(ndigits >= 2 ? ndigits : 2); 168 | #endif 169 | return digits(); 170 | } 171 | 172 | /** 173 | * @return the number of decimal digits of precision in a real number. 174 | **********************************************************************/ 175 | static int digits10() { 176 | #if GEOGRAPHICLIB_PRECISION != 5 177 | return std::numeric_limits::digits10; 178 | #else 179 | return std::numeric_limits::digits10(); 180 | #endif 181 | } 182 | 183 | /** 184 | * Number of additional decimal digits of precision for real relative to 185 | * double (0 for float). 186 | **********************************************************************/ 187 | static int extra_digits() { 188 | return 189 | digits10() > std::numeric_limits::digits10 ? 190 | digits10() - std::numeric_limits::digits10 : 0; 191 | } 192 | 193 | /** 194 | * true if the machine is big-endian. 195 | **********************************************************************/ 196 | static const bool bigendian = GEOGRAPHICLIB_WORDS_BIGENDIAN; 197 | 198 | /** 199 | * @tparam T the type of the returned value. 200 | * @return π. 201 | **********************************************************************/ 202 | template static T pi() { 203 | using std::atan2; 204 | static const T pi = atan2(T(0), T(-1)); 205 | return pi; 206 | } 207 | /** 208 | * A synonym for pi(). 209 | **********************************************************************/ 210 | static real pi() { return pi(); } 211 | 212 | /** 213 | * @tparam T the type of the returned value. 214 | * @return the number of radians in a degree. 215 | **********************************************************************/ 216 | template static T degree() { 217 | static const T degree = pi() / 180; 218 | return degree; 219 | } 220 | /** 221 | * A synonym for degree(). 222 | **********************************************************************/ 223 | static real degree() { return degree(); } 224 | 225 | /** 226 | * Square a number. 227 | * 228 | * @tparam T the type of the argument and the returned value. 229 | * @param[in] x 230 | * @return x2. 231 | **********************************************************************/ 232 | template static T sq(T x) 233 | { return x * x; } 234 | 235 | /** 236 | * The hypotenuse function avoiding underflow and overflow. 237 | * 238 | * @tparam T the type of the arguments and the returned value. 239 | * @param[in] x 240 | * @param[in] y 241 | * @return sqrt(x2 + y2). 242 | **********************************************************************/ 243 | template static T hypot(T x, T y) { 244 | #if GEOGRAPHICLIB_CXX11_MATH 245 | using std::hypot; return hypot(x, y); 246 | #else 247 | using std::abs; using std::sqrt; 248 | x = abs(x); y = abs(y); 249 | if (x < y) std::swap(x, y); // Now x >= y >= 0 250 | y /= (x ? x : 1); 251 | return x * sqrt(1 + y * y); 252 | // For an alternative (square-root free) method see 253 | // C. Moler and D. Morrision (1983) https://doi.org/10.1147/rd.276.0577 254 | // and A. A. Dubrulle (1983) https://doi.org/10.1147/rd.276.0582 255 | #endif 256 | } 257 | 258 | /** 259 | * exp(\e x) − 1 accurate near \e x = 0. 260 | * 261 | * @tparam T the type of the argument and the returned value. 262 | * @param[in] x 263 | * @return exp(\e x) − 1. 264 | **********************************************************************/ 265 | template static T expm1(T x) { 266 | #if GEOGRAPHICLIB_CXX11_MATH 267 | using std::expm1; return expm1(x); 268 | #else 269 | using std::exp; using std::abs; using std::log; 270 | GEOGRAPHICLIB_VOLATILE T 271 | y = exp(x), 272 | z = y - 1; 273 | // The reasoning here is similar to that for log1p. The expression 274 | // mathematically reduces to exp(x) - 1, and the factor z/log(y) = (y - 275 | // 1)/log(y) is a slowly varying quantity near y = 1 and is accurately 276 | // computed. 277 | return abs(x) > 1 ? z : (z == 0 ? x : x * z / log(y)); 278 | #endif 279 | } 280 | 281 | /** 282 | * log(1 + \e x) accurate near \e x = 0. 283 | * 284 | * @tparam T the type of the argument and the returned value. 285 | * @param[in] x 286 | * @return log(1 + \e x). 287 | **********************************************************************/ 288 | template static T log1p(T x) { 289 | #if GEOGRAPHICLIB_CXX11_MATH 290 | using std::log1p; return log1p(x); 291 | #else 292 | using std::log; 293 | GEOGRAPHICLIB_VOLATILE T 294 | y = 1 + x, 295 | z = y - 1; 296 | // Here's the explanation for this magic: y = 1 + z, exactly, and z 297 | // approx x, thus log(y)/z (which is nearly constant near z = 0) returns 298 | // a good approximation to the true log(1 + x)/x. The multiplication x * 299 | // (log(y)/z) introduces little additional error. 300 | return z == 0 ? x : x * log(y) / z; 301 | #endif 302 | } 303 | 304 | /** 305 | * The inverse hyperbolic sine function. 306 | * 307 | * @tparam T the type of the argument and the returned value. 308 | * @param[in] x 309 | * @return asinh(\e x). 310 | **********************************************************************/ 311 | template static T asinh(T x) { 312 | #if GEOGRAPHICLIB_CXX11_MATH 313 | using std::asinh; return asinh(x); 314 | #else 315 | using std::abs; T y = abs(x); // Enforce odd parity 316 | y = log1p(y * (1 + y/(hypot(T(1), y) + 1))); 317 | return x < 0 ? -y : y; 318 | #endif 319 | } 320 | 321 | /** 322 | * The inverse hyperbolic tangent function. 323 | * 324 | * @tparam T the type of the argument and the returned value. 325 | * @param[in] x 326 | * @return atanh(\e x). 327 | **********************************************************************/ 328 | template static T atanh(T x) { 329 | #if GEOGRAPHICLIB_CXX11_MATH 330 | using std::atanh; return atanh(x); 331 | #else 332 | using std::abs; T y = abs(x); // Enforce odd parity 333 | y = log1p(2 * y/(1 - y))/2; 334 | return x < 0 ? -y : y; 335 | #endif 336 | } 337 | 338 | /** 339 | * The cube root function. 340 | * 341 | * @tparam T the type of the argument and the returned value. 342 | * @param[in] x 343 | * @return the real cube root of \e x. 344 | **********************************************************************/ 345 | template static T cbrt(T x) { 346 | #if GEOGRAPHICLIB_CXX11_MATH 347 | using std::cbrt; return cbrt(x); 348 | #else 349 | using std::abs; using std::pow; 350 | T y = pow(abs(x), 1/T(3)); // Return the real cube root 351 | return x < 0 ? -y : y; 352 | #endif 353 | } 354 | 355 | /** 356 | * Fused multiply and add. 357 | * 358 | * @tparam T the type of the arguments and the returned value. 359 | * @param[in] x 360 | * @param[in] y 361 | * @param[in] z 362 | * @return xy + z, correctly rounded (on those platforms with 363 | * support for the fma instruction). 364 | * 365 | * On platforms without the fma instruction, no attempt is 366 | * made to improve on the result of a rounded multiplication followed by a 367 | * rounded addition. 368 | **********************************************************************/ 369 | template static T fma(T x, T y, T z) { 370 | #if GEOGRAPHICLIB_CXX11_MATH 371 | using std::fma; return fma(x, y, z); 372 | #else 373 | return x * y + z; 374 | #endif 375 | } 376 | 377 | /** 378 | * Normalize a two-vector. 379 | * 380 | * @tparam T the type of the argument and the returned value. 381 | * @param[in,out] x on output set to x/hypot(x, y). 382 | * @param[in,out] y on output set to y/hypot(x, y). 383 | **********************************************************************/ 384 | template static void norm(T& x, T& y) 385 | { T h = hypot(x, y); x /= h; y /= h; } 386 | 387 | /** 388 | * The error-free sum of two numbers. 389 | * 390 | * @tparam T the type of the argument and the returned value. 391 | * @param[in] u 392 | * @param[in] v 393 | * @param[out] t the exact error given by (\e u + \e v) - \e s. 394 | * @return \e s = round(\e u + \e v). 395 | * 396 | * See D. E. Knuth, TAOCP, Vol 2, 4.2.2, Theorem B. (Note that \e t can be 397 | * the same as one of the first two arguments.) 398 | **********************************************************************/ 399 | template static T sum(T u, T v, T& t) { 400 | GEOGRAPHICLIB_VOLATILE T s = u + v; 401 | GEOGRAPHICLIB_VOLATILE T up = s - v; 402 | GEOGRAPHICLIB_VOLATILE T vpp = s - up; 403 | up -= u; 404 | vpp -= v; 405 | t = -(up + vpp); 406 | // u + v = s + t 407 | // = round(u + v) + t 408 | return s; 409 | } 410 | 411 | /** 412 | * Evaluate a polynomial. 413 | * 414 | * @tparam T the type of the arguments and returned value. 415 | * @param[in] N the order of the polynomial. 416 | * @param[in] p the coefficient array (of size \e N + 1). 417 | * @param[in] x the variable. 418 | * @return the value of the polynomial. 419 | * 420 | * Evaluate y = ∑n=0..N 421 | * pn xN−n. 422 | * Return 0 if \e N < 0. Return p0, if \e N = 0 (even 423 | * if \e x is infinite or a nan). The evaluation uses Horner's method. 424 | **********************************************************************/ 425 | template static T polyval(int N, const T p[], T x) 426 | // This used to employ Math::fma; but that's too slow and it seemed not to 427 | // improve the accuracy noticeably. This might change when there's direct 428 | // hardware support for fma. 429 | { T y = N < 0 ? 0 : *p++; while (--N >= 0) y = y * x + *p++; return y; } 430 | 431 | /** 432 | * Normalize an angle. 433 | * 434 | * @tparam T the type of the argument and returned value. 435 | * @param[in] x the angle in degrees. 436 | * @return the angle reduced to the range([−180°, 180°]. 437 | * 438 | * The range of \e x is unrestricted. 439 | **********************************************************************/ 440 | template static T AngNormalize(T x) { 441 | #if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION != 4 442 | using std::remainder; 443 | x = remainder(x, T(360)); return x != -180 ? x : 180; 444 | #else 445 | using std::fmod; 446 | T y = fmod(x, T(360)); 447 | #if defined(_MSC_VER) && _MSC_VER < 1900 448 | // Before version 14 (2015), Visual Studio had problems dealing 449 | // with -0.0. Specifically 450 | // VC 10,11,12 and 32-bit compile: fmod(-0.0, 360.0) -> +0.0 451 | // sincosd has a similar fix. 452 | // python 2.7 on Windows 32-bit machines has the same problem. 453 | if (x == 0) y = x; 454 | #endif 455 | return y <= -180 ? y + 360 : (y <= 180 ? y : y - 360); 456 | #endif 457 | } 458 | 459 | /** 460 | * Normalize a latitude. 461 | * 462 | * @tparam T the type of the argument and returned value. 463 | * @param[in] x the angle in degrees. 464 | * @return x if it is in the range [−90°, 90°], otherwise 465 | * return NaN. 466 | **********************************************************************/ 467 | template static T LatFix(T x) 468 | { using std::abs; return abs(x) > 90 ? NaN() : x; } 469 | 470 | /** 471 | * The exact difference of two angles reduced to 472 | * (−180°, 180°]. 473 | * 474 | * @tparam T the type of the arguments and returned value. 475 | * @param[in] x the first angle in degrees. 476 | * @param[in] y the second angle in degrees. 477 | * @param[out] e the error term in degrees. 478 | * @return \e d, the truncated value of \e y − \e x. 479 | * 480 | * This computes \e z = \e y − \e x exactly, reduced to 481 | * (−180°, 180°]; and then sets \e z = \e d + \e e where \e d 482 | * is the nearest representable number to \e z and \e e is the truncation 483 | * error. If \e d = −180, then \e e > 0; If \e d = 180, then \e e 484 | * ≤ 0. 485 | **********************************************************************/ 486 | template static T AngDiff(T x, T y, T& e) { 487 | #if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION != 4 488 | using std::remainder; 489 | T t, d = AngNormalize(sum(remainder(-x, T(360)), 490 | remainder( y, T(360)), t)); 491 | #else 492 | T t, d = AngNormalize(sum(AngNormalize(-x), AngNormalize(y), t)); 493 | #endif 494 | // Here y - x = d + t (mod 360), exactly, where d is in (-180,180] and 495 | // abs(t) <= eps (eps = 2^-45 for doubles). The only case where the 496 | // addition of t takes the result outside the range (-180,180] is d = 180 497 | // and t > 0. The case, d = -180 + eps, t = -eps, can't happen, since 498 | // sum would have returned the exact result in such a case (i.e., given t 499 | // = 0). 500 | return sum(d == 180 && t > 0 ? -180 : d, t, e); 501 | } 502 | 503 | /** 504 | * Difference of two angles reduced to [−180°, 180°] 505 | * 506 | * @tparam T the type of the arguments and returned value. 507 | * @param[in] x the first angle in degrees. 508 | * @param[in] y the second angle in degrees. 509 | * @return \e y − \e x, reduced to the range [−180°, 510 | * 180°]. 511 | * 512 | * The result is equivalent to computing the difference exactly, reducing 513 | * it to (−180°, 180°] and rounding the result. Note that 514 | * this prescription allows −180° to be returned (e.g., if \e x 515 | * is tiny and negative and \e y = 180°). 516 | **********************************************************************/ 517 | template static T AngDiff(T x, T y) 518 | { T e; return AngDiff(x, y, e); } 519 | 520 | /** 521 | * Coarsen a value close to zero. 522 | * 523 | * @tparam T the type of the argument and returned value. 524 | * @param[in] x 525 | * @return the coarsened value. 526 | * 527 | * The makes the smallest gap in \e x = 1/16 - nextafter(1/16, 0) = 528 | * 1/257 for reals = 0.7 pm on the earth if \e x is an angle in 529 | * degrees. (This is about 1000 times more resolution than we get with 530 | * angles around 90°.) We use this to avoid having to deal with near 531 | * singular cases when \e x is non-zero but tiny (e.g., 532 | * 10−200). This converts -0 to +0; however tiny negative 533 | * numbers get converted to -0. 534 | **********************************************************************/ 535 | template static T AngRound(T x) { 536 | using std::abs; 537 | static const T z = 1/T(16); 538 | if (x == 0) return 0; 539 | GEOGRAPHICLIB_VOLATILE T y = abs(x); 540 | // The compiler mustn't "simplify" z - (z - y) to y 541 | y = y < z ? z - (z - y) : y; 542 | return x < 0 ? -y : y; 543 | } 544 | 545 | /** 546 | * Evaluate the sine and cosine function with the argument in degrees 547 | * 548 | * @tparam T the type of the arguments. 549 | * @param[in] x in degrees. 550 | * @param[out] sinx sin(x). 551 | * @param[out] cosx cos(x). 552 | * 553 | * The results obey exactly the elementary properties of the trigonometric 554 | * functions, e.g., sin 9° = cos 81° = − sin 123456789°. 555 | * If x = −0, then \e sinx = −0; this is the only case where 556 | * −0 is returned. 557 | **********************************************************************/ 558 | template static void sincosd(T x, T& sinx, T& cosx) { 559 | // In order to minimize round-off errors, this function exactly reduces 560 | // the argument to the range [-45, 45] before converting it to radians. 561 | using std::sin; using std::cos; 562 | T r; int q; 563 | #if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION <= 3 && \ 564 | !defined(__GNUC__) 565 | // Disable for gcc because of bug in glibc version < 2.22, see 566 | // https://sourceware.org/bugzilla/show_bug.cgi?id=17569 567 | // Once this fix is widely deployed, should insert a runtime test for the 568 | // glibc version number. For example 569 | // #include 570 | // std::string version(gnu_get_libc_version()); => "2.22" 571 | using std::remquo; 572 | r = remquo(x, T(90), &q); 573 | #else 574 | using std::fmod; using std::floor; 575 | r = fmod(x, T(360)); 576 | q = int(floor(r / 90 + T(0.5))); 577 | r -= 90 * q; 578 | #endif 579 | // now abs(r) <= 45 580 | r *= degree(); 581 | // Possibly could call the gnu extension sincos 582 | T s = sin(r), c = cos(r); 583 | #if defined(_MSC_VER) && _MSC_VER < 1900 584 | // Before version 14 (2015), Visual Studio had problems dealing 585 | // with -0.0. Specifically 586 | // VC 10,11,12 and 32-bit compile: fmod(-0.0, 360.0) -> +0.0 587 | // VC 12 and 64-bit compile: sin(-0.0) -> +0.0 588 | // AngNormalize has a similar fix. 589 | // python 2.7 on Windows 32-bit machines has the same problem. 590 | if (x == 0) s = x; 591 | #endif 592 | switch (unsigned(q) & 3U) { 593 | case 0U: sinx = s; cosx = c; break; 594 | case 1U: sinx = c; cosx = -s; break; 595 | case 2U: sinx = -s; cosx = -c; break; 596 | default: sinx = -c; cosx = s; break; // case 3U 597 | } 598 | // Set sign of 0 results. -0 only produced for sin(-0) 599 | if (x != 0) { sinx += T(0); cosx += T(0); } 600 | } 601 | 602 | /** 603 | * Evaluate the sine function with the argument in degrees 604 | * 605 | * @tparam T the type of the argument and the returned value. 606 | * @param[in] x in degrees. 607 | * @return sin(x). 608 | **********************************************************************/ 609 | template static T sind(T x) { 610 | // See sincosd 611 | using std::sin; using std::cos; 612 | T r; int q; 613 | #if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION <= 3 && \ 614 | !defined(__GNUC__) 615 | using std::remquo; 616 | r = remquo(x, T(90), &q); 617 | #else 618 | using std::fmod; using std::floor; 619 | r = fmod(x, T(360)); 620 | q = int(floor(r / 90 + T(0.5))); 621 | r -= 90 * q; 622 | #endif 623 | // now abs(r) <= 45 624 | r *= degree(); 625 | unsigned p = unsigned(q); 626 | r = p & 1U ? cos(r) : sin(r); 627 | if (p & 2U) r = -r; 628 | if (x != 0) r += T(0); 629 | return r; 630 | } 631 | 632 | /** 633 | * Evaluate the cosine function with the argument in degrees 634 | * 635 | * @tparam T the type of the argument and the returned value. 636 | * @param[in] x in degrees. 637 | * @return cos(x). 638 | **********************************************************************/ 639 | template static T cosd(T x) { 640 | // See sincosd 641 | using std::sin; using std::cos; 642 | T r; int q; 643 | #if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION <= 3 && \ 644 | !defined(__GNUC__) 645 | using std::remquo; 646 | r = remquo(x, T(90), &q); 647 | #else 648 | using std::fmod; using std::floor; 649 | r = fmod(x, T(360)); 650 | q = int(floor(r / 90 + T(0.5))); 651 | r -= 90 * q; 652 | #endif 653 | // now abs(r) <= 45 654 | r *= degree(); 655 | unsigned p = unsigned(q + 1); 656 | r = p & 1U ? cos(r) : sin(r); 657 | if (p & 2U) r = -r; 658 | return T(0) + r; 659 | } 660 | 661 | /** 662 | * Evaluate the tangent function with the argument in degrees 663 | * 664 | * @tparam T the type of the argument and the returned value. 665 | * @param[in] x in degrees. 666 | * @return tan(x). 667 | * 668 | * If \e x = ±90°, then a suitably large (but finite) value is 669 | * returned. 670 | **********************************************************************/ 671 | template static T tand(T x) { 672 | static const T overflow = 1 / sq(std::numeric_limits::epsilon()); 673 | T s, c; 674 | sincosd(x, s, c); 675 | return c != 0 ? s / c : (s < 0 ? -overflow : overflow); 676 | } 677 | 678 | /** 679 | * Evaluate the atan2 function with the result in degrees 680 | * 681 | * @tparam T the type of the arguments and the returned value. 682 | * @param[in] y 683 | * @param[in] x 684 | * @return atan2(y, x) in degrees. 685 | * 686 | * The result is in the range (−180° 180°]. N.B., 687 | * atan2d(±0, −1) = +180°; atan2d(−ε, 688 | * −1) = −180°, for ε positive and tiny; 689 | * atan2d(±0, +1) = ±0°. 690 | **********************************************************************/ 691 | template static T atan2d(T y, T x) { 692 | // In order to minimize round-off errors, this function rearranges the 693 | // arguments so that result of atan2 is in the range [-pi/4, pi/4] before 694 | // converting it to degrees and mapping the result to the correct 695 | // quadrant. 696 | using std::atan2; using std::abs; 697 | int q = 0; 698 | if (abs(y) > abs(x)) { std::swap(x, y); q = 2; } 699 | if (x < 0) { x = -x; ++q; } 700 | // here x >= 0 and x >= abs(y), so angle is in [-pi/4, pi/4] 701 | T ang = atan2(y, x) / degree(); 702 | switch (q) { 703 | // Note that atan2d(-0.0, 1.0) will return -0. However, we expect that 704 | // atan2d will not be called with y = -0. If need be, include 705 | // 706 | // case 0: ang = 0 + ang; break; 707 | // 708 | // and handle mpfr as in AngRound. 709 | case 1: ang = (y >= 0 ? 180 : -180) - ang; break; 710 | case 2: ang = 90 - ang; break; 711 | case 3: ang = -90 + ang; break; 712 | } 713 | return ang; 714 | } 715 | 716 | /** 717 | * Evaluate the atan function with the result in degrees 718 | * 719 | * @tparam T the type of the argument and the returned value. 720 | * @param[in] x 721 | * @return atan(x) in degrees. 722 | **********************************************************************/ 723 | template static T atand(T x) 724 | { return atan2d(x, T(1)); } 725 | 726 | /** 727 | * Evaluate e atanh(e x) 728 | * 729 | * @tparam T the type of the argument and the returned value. 730 | * @param[in] x 731 | * @param[in] es the signed eccentricity = sign(e2) 732 | * sqrt(|e2|) 733 | * @return e atanh(e x) 734 | * 735 | * If e2 is negative (e is imaginary), the 736 | * expression is evaluated in terms of atan. 737 | **********************************************************************/ 738 | template static T eatanhe(T x, T es); 739 | 740 | /** 741 | * Copy the sign. 742 | * 743 | * @tparam T the type of the argument. 744 | * @param[in] x gives the magitude of the result. 745 | * @param[in] y gives the sign of the result. 746 | * @return value with the magnitude of \e x and with the sign of \e y. 747 | * 748 | * This routine correctly handles the case \e y = −0, returning 749 | * &minus|x|. 750 | **********************************************************************/ 751 | template static T copysign(T x, T y) { 752 | #if GEOGRAPHICLIB_CXX11_MATH 753 | using std::copysign; return copysign(x, y); 754 | #else 755 | using std::abs; 756 | // NaN counts as positive 757 | return abs(x) * (y < 0 || (y == 0 && 1/y < 0) ? -1 : 1); 758 | #endif 759 | } 760 | 761 | /** 762 | * tanχ in terms of tanφ 763 | * 764 | * @tparam T the type of the argument and the returned value. 765 | * @param[in] tau τ = tanφ 766 | * @param[in] es the signed eccentricity = sign(e2) 767 | * sqrt(|e2|) 768 | * @return τ′ = tanχ 769 | * 770 | * See Eqs. (7--9) of 771 | * C. F. F. Karney, 772 | * 773 | * Transverse Mercator with an accuracy of a few nanometers, 774 | * J. Geodesy 85(8), 475--485 (Aug. 2011) 775 | * (preprint 776 | * arXiv:1002.1417). 777 | **********************************************************************/ 778 | template static T taupf(T tau, T es); 779 | 780 | /** 781 | * tanφ in terms of tanχ 782 | * 783 | * @tparam T the type of the argument and the returned value. 784 | * @param[in] taup τ′ = tanχ 785 | * @param[in] es the signed eccentricity = sign(e2) 786 | * sqrt(|e2|) 787 | * @return τ = tanφ 788 | * 789 | * See Eqs. (19--21) of 790 | * C. F. F. Karney, 791 | * 792 | * Transverse Mercator with an accuracy of a few nanometers, 793 | * J. Geodesy 85(8), 475--485 (Aug. 2011) 794 | * (preprint 795 | * arXiv:1002.1417). 796 | **********************************************************************/ 797 | template static T tauf(T taup, T es); 798 | 799 | /** 800 | * Test for finiteness. 801 | * 802 | * @tparam T the type of the argument. 803 | * @param[in] x 804 | * @return true if number is finite, false if NaN or infinite. 805 | **********************************************************************/ 806 | template static bool isfinite(T x) { 807 | #if GEOGRAPHICLIB_CXX11_MATH 808 | using std::isfinite; return isfinite(x); 809 | #else 810 | using std::abs; 811 | #if defined(_MSC_VER) 812 | return abs(x) <= (std::numeric_limits::max)(); 813 | #else 814 | // There's a problem using MPFR C++ 3.6.3 and g++ -std=c++14 (reported on 815 | // 2015-05-04) with the parens around std::numeric_limits::max. Of 816 | // course, these parens are only needed to deal with Windows stupidly 817 | // defining max as a macro. So don't insert the parens on non-Windows 818 | // platforms. 819 | return abs(x) <= std::numeric_limits::max(); 820 | #endif 821 | #endif 822 | } 823 | 824 | /** 825 | * The NaN (not a number) 826 | * 827 | * @tparam T the type of the returned value. 828 | * @return NaN if available, otherwise return the max real of type T. 829 | **********************************************************************/ 830 | template static T NaN() { 831 | #if defined(_MSC_VER) 832 | return std::numeric_limits::has_quiet_NaN ? 833 | std::numeric_limits::quiet_NaN() : 834 | (std::numeric_limits::max)(); 835 | #else 836 | return std::numeric_limits::has_quiet_NaN ? 837 | std::numeric_limits::quiet_NaN() : 838 | std::numeric_limits::max(); 839 | #endif 840 | } 841 | /** 842 | * A synonym for NaN(). 843 | **********************************************************************/ 844 | static real NaN() { return NaN(); } 845 | 846 | /** 847 | * Test for NaN. 848 | * 849 | * @tparam T the type of the argument. 850 | * @param[in] x 851 | * @return true if argument is a NaN. 852 | **********************************************************************/ 853 | template static bool isnan(T x) { 854 | #if GEOGRAPHICLIB_CXX11_MATH 855 | using std::isnan; return isnan(x); 856 | #else 857 | return x != x; 858 | #endif 859 | } 860 | 861 | /** 862 | * Infinity 863 | * 864 | * @tparam T the type of the returned value. 865 | * @return infinity if available, otherwise return the max real. 866 | **********************************************************************/ 867 | template static T infinity() { 868 | #if defined(_MSC_VER) 869 | return std::numeric_limits::has_infinity ? 870 | std::numeric_limits::infinity() : 871 | (std::numeric_limits::max)(); 872 | #else 873 | return std::numeric_limits::has_infinity ? 874 | std::numeric_limits::infinity() : 875 | std::numeric_limits::max(); 876 | #endif 877 | } 878 | /** 879 | * A synonym for infinity(). 880 | **********************************************************************/ 881 | static real infinity() { return infinity(); } 882 | 883 | /** 884 | * Swap the bytes of a quantity 885 | * 886 | * @tparam T the type of the argument and the returned value. 887 | * @param[in] x 888 | * @return x with its bytes swapped. 889 | **********************************************************************/ 890 | template static T swab(T x) { 891 | union { 892 | T r; 893 | unsigned char c[sizeof(T)]; 894 | } b; 895 | b.r = x; 896 | for (int i = sizeof(T)/2; i--; ) 897 | std::swap(b.c[i], b.c[sizeof(T) - 1 - i]); 898 | return b.r; 899 | } 900 | 901 | #if GEOGRAPHICLIB_PRECISION == 4 902 | typedef boost::math::policies::policy 903 | < boost::math::policies::domain_error 904 | , 905 | boost::math::policies::pole_error 906 | , 907 | boost::math::policies::overflow_error 908 | , 909 | boost::math::policies::evaluation_error 910 | > 911 | boost_special_functions_policy; 912 | 913 | static real hypot(real x, real y) 914 | { return boost::math::hypot(x, y, boost_special_functions_policy()); } 915 | 916 | static real expm1(real x) 917 | { return boost::math::expm1(x, boost_special_functions_policy()); } 918 | 919 | static real log1p(real x) 920 | { return boost::math::log1p(x, boost_special_functions_policy()); } 921 | 922 | static real asinh(real x) 923 | { return boost::math::asinh(x, boost_special_functions_policy()); } 924 | 925 | static real atanh(real x) 926 | { return boost::math::atanh(x, boost_special_functions_policy()); } 927 | 928 | static real cbrt(real x) 929 | { return boost::math::cbrt(x, boost_special_functions_policy()); } 930 | 931 | static real fma(real x, real y, real z) 932 | { return fmaq(__float128(x), __float128(y), __float128(z)); } 933 | 934 | static real copysign(real x, real y) 935 | { return boost::math::copysign(x, y); } 936 | 937 | static bool isnan(real x) { return boost::math::isnan(x); } 938 | 939 | static bool isfinite(real x) { return boost::math::isfinite(x); } 940 | #endif 941 | }; 942 | 943 | } // namespace GeographicLib 944 | 945 | #endif // GEOGRAPHICLIB_MATH_HPP 946 | -------------------------------------------------------------------------------- /3rd/GeographicLib/src/Geocentric.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file Geocentric.cpp 3 | * \brief Implementation for GeographicLib::Geocentric class 4 | * 5 | * Copyright (c) Charles Karney (2008-2017) and licensed 6 | * under the MIT/X11 License. For more information, see 7 | * https://geographiclib.sourceforge.io/ 8 | **********************************************************************/ 9 | 10 | #include "Geocentric/Geocentric.hpp" 11 | 12 | namespace GeographicLib { 13 | 14 | using namespace std; 15 | 16 | Geocentric::Geocentric(real a, real f) 17 | : _a(a) 18 | , _f(f) 19 | , _e2(_f * (2 - _f)) 20 | , _e2m(Math::sq(1 - _f)) // 1 - _e2 21 | , _e2a(abs(_e2)) 22 | , _e4a(Math::sq(_e2)) 23 | , _maxrad(2 * _a / numeric_limits::epsilon()) 24 | { 25 | if (!(Math::isfinite(_a) && _a > 0)) 26 | throw GeographicErr("Equatorial radius is not positive"); 27 | if (!(Math::isfinite(_f) && _f < 1)) 28 | throw GeographicErr("Polar semi-axis is not positive"); 29 | } 30 | 31 | const Geocentric& Geocentric::WGS84() { 32 | static const Geocentric wgs84(Constants::WGS84_a(), Constants::WGS84_f()); 33 | return wgs84; 34 | } 35 | 36 | void Geocentric::IntForward(real lat, real lon, real h, 37 | real& X, real& Y, real& Z, 38 | real M[dim2_]) const { 39 | real sphi, cphi, slam, clam; 40 | Math::sincosd(Math::LatFix(lat), sphi, cphi); 41 | Math::sincosd(lon, slam, clam); 42 | real n = _a/sqrt(1 - _e2 * Math::sq(sphi)); 43 | Z = (_e2m * n + h) * sphi; 44 | X = (n + h) * cphi; 45 | Y = X * slam; 46 | X *= clam; 47 | if (M) 48 | Rotation(sphi, cphi, slam, clam, M); 49 | } 50 | 51 | void Geocentric::IntReverse(real X, real Y, real Z, 52 | real& lat, real& lon, real& h, 53 | real M[dim2_]) const { 54 | real 55 | R = Math::hypot(X, Y), 56 | slam = R != 0 ? Y / R : 0, 57 | clam = R != 0 ? X / R : 1; 58 | h = Math::hypot(R, Z); // Distance to center of earth 59 | real sphi, cphi; 60 | if (h > _maxrad) { 61 | // We really far away (> 12 million light years); treat the earth as a 62 | // point and h, above, is an acceptable approximation to the height. 63 | // This avoids overflow, e.g., in the computation of disc below. It's 64 | // possible that h has overflowed to inf; but that's OK. 65 | // 66 | // Treat the case X, Y finite, but R overflows to +inf by scaling by 2. 67 | R = Math::hypot(X/2, Y/2); 68 | slam = R != 0 ? (Y/2) / R : 0; 69 | clam = R != 0 ? (X/2) / R : 1; 70 | real H = Math::hypot(Z/2, R); 71 | sphi = (Z/2) / H; 72 | cphi = R / H; 73 | } else if (_e4a == 0) { 74 | // Treat the spherical case. Dealing with underflow in the general case 75 | // with _e2 = 0 is difficult. Origin maps to N pole same as with 76 | // ellipsoid. 77 | real H = Math::hypot(h == 0 ? 1 : Z, R); 78 | sphi = (h == 0 ? 1 : Z) / H; 79 | cphi = R / H; 80 | h -= _a; 81 | } else { 82 | // Treat prolate spheroids by swapping R and Z here and by switching 83 | // the arguments to phi = atan2(...) at the end. 84 | real 85 | p = Math::sq(R / _a), 86 | q = _e2m * Math::sq(Z / _a), 87 | r = (p + q - _e4a) / 6; 88 | if (_f < 0) swap(p, q); 89 | if ( !(_e4a * q == 0 && r <= 0) ) { 90 | real 91 | // Avoid possible division by zero when r = 0 by multiplying 92 | // equations for s and t by r^3 and r, resp. 93 | S = _e4a * p * q / 4, // S = r^3 * s 94 | r2 = Math::sq(r), 95 | r3 = r * r2, 96 | disc = S * (2 * r3 + S); 97 | real u = r; 98 | if (disc >= 0) { 99 | real T3 = S + r3; 100 | // Pick the sign on the sqrt to maximize abs(T3). This minimizes 101 | // loss of precision due to cancellation. The result is unchanged 102 | // because of the way the T is used in definition of u. 103 | T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); // T3 = (r * t)^3 104 | // N.B. cbrt always returns the real root. cbrt(-8) = -2. 105 | real T = Math::cbrt(T3); // T = r * t 106 | // T can be zero; but then r2 / T -> 0. 107 | u += T + (T != 0 ? r2 / T : 0); 108 | } else { 109 | // T is complex, but the way u is defined the result is real. 110 | real ang = atan2(sqrt(-disc), -(S + r3)); 111 | // There are three possible cube roots. We choose the root which 112 | // avoids cancellation. Note that disc < 0 implies that r < 0. 113 | u += 2 * r * cos(ang / 3); 114 | } 115 | real 116 | v = sqrt(Math::sq(u) + _e4a * q), // guaranteed positive 117 | // Avoid loss of accuracy when u < 0. Underflow doesn't occur in 118 | // e4 * q / (v - u) because u ~ e^4 when q is small and u < 0. 119 | uv = u < 0 ? _e4a * q / (v - u) : u + v, // u+v, guaranteed positive 120 | // Need to guard against w going negative due to roundoff in uv - q. 121 | w = max(real(0), _e2a * (uv - q) / (2 * v)), 122 | // Rearrange expression for k to avoid loss of accuracy due to 123 | // subtraction. Division by 0 not possible because uv > 0, w >= 0. 124 | k = uv / (sqrt(uv + Math::sq(w)) + w), 125 | k1 = _f >= 0 ? k : k - _e2, 126 | k2 = _f >= 0 ? k + _e2 : k, 127 | d = k1 * R / k2, 128 | H = Math::hypot(Z/k1, R/k2); 129 | sphi = (Z/k1) / H; 130 | cphi = (R/k2) / H; 131 | h = (1 - _e2m/k1) * Math::hypot(d, Z); 132 | } else { // e4 * q == 0 && r <= 0 133 | // This leads to k = 0 (oblate, equatorial plane) and k + e^2 = 0 134 | // (prolate, rotation axis) and the generation of 0/0 in the general 135 | // formulas for phi and h. using the general formula and division by 0 136 | // in formula for h. So handle this case by taking the limits: 137 | // f > 0: z -> 0, k -> e2 * sqrt(q)/sqrt(e4 - p) 138 | // f < 0: R -> 0, k + e2 -> - e2 * sqrt(q)/sqrt(e4 - p) 139 | real 140 | zz = sqrt((_f >= 0 ? _e4a - p : p) / _e2m), 141 | xx = sqrt( _f < 0 ? _e4a - p : p ), 142 | H = Math::hypot(zz, xx); 143 | sphi = zz / H; 144 | cphi = xx / H; 145 | if (Z < 0) sphi = -sphi; // for tiny negative Z (not for prolate) 146 | h = - _a * (_f >= 0 ? _e2m : 1) * H / _e2a; 147 | } 148 | } 149 | lat = Math::atan2d(sphi, cphi); 150 | lon = Math::atan2d(slam, clam); 151 | if (M) 152 | Rotation(sphi, cphi, slam, clam, M); 153 | } 154 | 155 | void Geocentric::Rotation(real sphi, real cphi, real slam, real clam, 156 | real M[dim2_]) { 157 | // This rotation matrix is given by the following quaternion operations 158 | // qrot(lam, [0,0,1]) * qrot(phi, [0,-1,0]) * [1,1,1,1]/2 159 | // or 160 | // qrot(pi/2 + lam, [0,0,1]) * qrot(-pi/2 + phi , [-1,0,0]) 161 | // where 162 | // qrot(t,v) = [cos(t/2), sin(t/2)*v[1], sin(t/2)*v[2], sin(t/2)*v[3]] 163 | 164 | // Local X axis (east) in geocentric coords 165 | M[0] = -slam; M[3] = clam; M[6] = 0; 166 | // Local Y axis (north) in geocentric coords 167 | M[1] = -clam * sphi; M[4] = -slam * sphi; M[7] = cphi; 168 | // Local Z axis (up) in geocentric coords 169 | M[2] = clam * cphi; M[5] = slam * cphi; M[8] = sphi; 170 | } 171 | 172 | } // namespace GeographicLib 173 | -------------------------------------------------------------------------------- /3rd/GeographicLib/src/LocalCartesian.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file LocalCartesian.cpp 3 | * \brief Implementation for GeographicLib::LocalCartesian class 4 | * 5 | * Copyright (c) Charles Karney (2008-2015) and licensed 6 | * under the MIT/X11 License. For more information, see 7 | * https://geographiclib.sourceforge.io/ 8 | **********************************************************************/ 9 | 10 | #include "Geocentric/LocalCartesian.hpp" 11 | 12 | namespace GeographicLib { 13 | 14 | using namespace std; 15 | 16 | void LocalCartesian::Reset(real lat0, real lon0, real h0) { 17 | _lat0 = Math::LatFix(lat0); 18 | _lon0 = Math::AngNormalize(lon0); 19 | _h0 = h0; 20 | _earth.Forward(_lat0, _lon0, _h0, _x0, _y0, _z0); 21 | real sphi, cphi, slam, clam; 22 | Math::sincosd(_lat0, sphi, cphi); 23 | Math::sincosd(_lon0, slam, clam); 24 | Geocentric::Rotation(sphi, cphi, slam, clam, _r); 25 | } 26 | 27 | void LocalCartesian::MatrixMultiply(real M[dim2_]) const { 28 | // M = r' . M 29 | real t[dim2_]; 30 | copy(M, M + dim2_, t); 31 | for (size_t i = 0; i < dim2_; ++i) { 32 | size_t row = i / dim_, col = i % dim_; 33 | M[i] = _r[row] * t[col] + _r[row+3] * t[col+3] + _r[row+6] * t[col+6]; 34 | } 35 | } 36 | 37 | void LocalCartesian::IntForward(real lat, real lon, real h, 38 | real& x, real& y, real& z, 39 | real M[dim2_]) const { 40 | real xc, yc, zc; 41 | _earth.IntForward(lat, lon, h, xc, yc, zc, M); 42 | xc -= _x0; yc -= _y0; zc -= _z0; 43 | x = _r[0] * xc + _r[3] * yc + _r[6] * zc; 44 | y = _r[1] * xc + _r[4] * yc + _r[7] * zc; 45 | z = _r[2] * xc + _r[5] * yc + _r[8] * zc; 46 | if (M) 47 | MatrixMultiply(M); 48 | } 49 | 50 | void LocalCartesian::IntReverse(real x, real y, real z, 51 | real& lat, real& lon, real& h, 52 | real M[dim2_]) const { 53 | real 54 | xc = _x0 + _r[0] * x + _r[1] * y + _r[2] * z, 55 | yc = _y0 + _r[3] * x + _r[4] * y + _r[5] * z, 56 | zc = _z0 + _r[6] * x + _r[7] * y + _r[8] * z; 57 | _earth.IntReverse(xc, yc, zc, lat, lon, h, M); 58 | if (M) 59 | MatrixMultiply(M); 60 | } 61 | 62 | } // namespace GeographicLib 63 | -------------------------------------------------------------------------------- /3rd/GeographicLib/src/Math.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file Math.cpp 3 | * \brief Implementation for GeographicLib::Math class 4 | * 5 | * Copyright (c) Charles Karney (2015) and licensed 6 | * under the MIT/X11 License. For more information, see 7 | * https://geographiclib.sourceforge.io/ 8 | **********************************************************************/ 9 | 10 | #include "Geocentric/Math.hpp" 11 | 12 | #if defined(_MSC_VER) 13 | // Squelch warnings about constant conditional expressions 14 | # pragma warning (disable: 4127) 15 | #endif 16 | 17 | namespace GeographicLib { 18 | 19 | using namespace std; 20 | 21 | template T Math::eatanhe(T x, T es) { 22 | return es > T(0) ? es * atanh(es * x) : -es * atan(es * x); 23 | } 24 | 25 | template T Math::taupf(T tau, T es) { 26 | T tau1 = hypot(T(1), tau), 27 | sig = sinh( eatanhe(tau / tau1, es ) ); 28 | return hypot(T(1), sig) * tau - sig * tau1; 29 | } 30 | 31 | template T Math::tauf(T taup, T es) { 32 | static const int numit = 5; 33 | static const T tol = sqrt(numeric_limits::epsilon()) / T(10); 34 | T e2m = T(1) - sq(es), 35 | // To lowest order in e^2, taup = (1 - e^2) * tau = _e2m * tau; so use 36 | // tau = taup/_e2m as a starting guess. (This starting guess is the 37 | // geocentric latitude which, to first order in the flattening, is equal 38 | // to the conformal latitude.) Only 1 iteration is needed for |lat| < 39 | // 3.35 deg, otherwise 2 iterations are needed. If, instead, tau = taup 40 | // is used the mean number of iterations increases to 1.99 (2 iterations 41 | // are needed except near tau = 0). 42 | tau = taup/e2m, 43 | stol = tol * max(T(1), abs(taup)); 44 | // min iterations = 1, max iterations = 2; mean = 1.94 45 | for (int i = 0; i < numit || GEOGRAPHICLIB_PANIC; ++i) { 46 | T taupa = taupf(tau, es), 47 | dtau = (taup - taupa) * (1 + e2m * sq(tau)) / 48 | ( e2m * hypot(T(1), tau) * hypot(T(1), taupa) ); 49 | tau += dtau; 50 | if (!(abs(dtau) >= stol)) 51 | break; 52 | } 53 | return tau; 54 | } 55 | 56 | /// \cond SKIP 57 | // Instantiate 58 | template Math::real Math::eatanhe(Math::real, Math::real); 59 | template Math::real Math::taupf(Math::real, Math::real); 60 | template Math::real Math::tauf(Math::real, Math::real); 61 | /// \endcond 62 | 63 | } // namespace GeographicLib 64 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(gps_imu_fusion) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(CMAKE_CXX_STANDARD 11) 6 | 7 | set(ALL_TARGET_LIBRARIES "") 8 | 9 | include(cmake/eigen.cmake) 10 | include(cmake/YAML.cmake) 11 | 12 | include(cmake/glog.cmake) 13 | include_directories(${GLOG_INCLUDE_DIRS}) 14 | 15 | add_subdirectory(3rd/GeographicLib GeographicLib) 16 | 17 | include_directories(${PROJECT_SOURCE_DIR}/3rd/GeographicLib/include/) 18 | include_directories(${PROJECT_SOURCE_DIR}/include) 19 | 20 | add_library(DEPS SHARED 21 | src/eskf.cpp 22 | src/gps_tool.cpp 23 | src/imu_tool.cpp 24 | src/eskf_flow.cpp 25 | src/observability_analysis.cpp 26 | ) 27 | 28 | target_link_libraries(DEPS 29 | ${ALL_TARGET_LIBRARIES} 30 | ${GLOG_LIBRARIES} 31 | libGeographiccc 32 | ) 33 | 34 | add_executable(gps_imu_fusion app/gps_imu_fusion.cpp) 35 | target_link_libraries(gps_imu_fusion DEPS) 36 | 37 | ################# TEST ################# 38 | add_executable(test_gps test/test_gps.cpp) 39 | target_link_libraries(test_gps DEPS) 40 | 41 | add_executable(test_imu test/test_imu.cpp) 42 | target_link_libraries(test_imu DEPS) 43 | 44 | add_executable(test_tool test/test_tool.cpp) 45 | target_link_libraries(test_tool DEPS) 46 | -------------------------------------------------------------------------------- /app/gps_imu_fusion.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/2/24. 3 | // 4 | #include "eskf_flow.h" 5 | 6 | int main(int argc, char **argv) { 7 | google::InitGoogleLogging(argv[0]); 8 | FLAGS_colorlogtostderr = true; 9 | FLAGS_alsologtostderr = true; 10 | 11 | if (argc != 3) { 12 | std::cout << "Please enter right command and parameters!" << std::endl; 13 | 14 | return -1; 15 | } 16 | 17 | std::string config_file_path(argv[1]); 18 | std::string data_file_path(argv[2]); 19 | 20 | ESKFFlow eskf_flow(config_file_path, data_file_path); 21 | 22 | //使用该函数时相当于只使用IMU位姿解算 23 | // eskf_flow.TestRun();//only predict 24 | 25 | eskf_flow.Run(); 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /cmake/YAML.cmake: -------------------------------------------------------------------------------- 1 | find_package(PkgConfig REQUIRED) 2 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 3 | include_directories(${YAML_CPP_INCLUDEDIR}) 4 | list(APPEND ALL_TARGET_LIBRARIES ${YAML_CPP_LIBRARIES}) -------------------------------------------------------------------------------- /cmake/eigen.cmake: -------------------------------------------------------------------------------- 1 | find_package(Eigen3 REQUIRED) 2 | include_directories(${EIGEN3_INCLUDE_DIRS}) -------------------------------------------------------------------------------- /cmake/glog.cmake: -------------------------------------------------------------------------------- 1 | include(FindPackageHandleStandardArgs) 2 | 3 | set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog") 4 | 5 | if (WIN32) 6 | find_path(GLOG_INCLUDE_DIR glog/logging.h 7 | PATHS ${GLOG_ROOT_DIR}/src/windows) 8 | else () 9 | find_path(GLOG_INCLUDE_DIR glog/logging.h 10 | PATHS ${GLOG_ROOT_DIR}) 11 | endif () 12 | 13 | if (MSVC) 14 | find_library(GLOG_LIBRARY_RELEASE libglog_static 15 | PATHS ${GLOG_ROOT_DIR} 16 | PATH_SUFFIXES Release) 17 | 18 | find_library(GLOG_LIBRARY_DEBUG libglog_static 19 | PATHS ${GLOG_ROOT_DIR} 20 | PATH_SUFFIXES Debug) 21 | 22 | set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG}) 23 | else () 24 | find_library(GLOG_LIBRARY glog 25 | PATHS ${GLOG_ROOT_DIR} 26 | PATH_SUFFIXES lib lib64) 27 | endif () 28 | 29 | find_package_handle_standard_args(Glog DEFAULT_MSG GLOG_INCLUDE_DIR GLOG_LIBRARY) 30 | 31 | if (GLOG_FOUND) 32 | set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) 33 | set(GLOG_LIBRARIES ${GLOG_LIBRARY}) 34 | message(STATUS "Found glog (include: ${GLOG_INCLUDE_DIR}, library: ${GLOG_LIBRARY})") 35 | mark_as_advanced(GLOG_ROOT_DIR GLOG_LIBRARY_RELEASE GLOG_LIBRARY_DEBUG 36 | GLOG_LIBRARY GLOG_INCLUDE_DIR) 37 | endif () -------------------------------------------------------------------------------- /config/config.yaml: -------------------------------------------------------------------------------- 1 | # earth 2 | earth_rotation_speed: 7.2921151467e-05 # rad/s 3 | earth_gravity: 9.794444352667332154 4 | 5 | # reference point 6 | ref_longitude: 120.401989 7 | ref_latitude: 31.508183 8 | ref_altitude: 0.0 9 | 10 | # kalman prediction process std 11 | position_error_prior_std: 1.0e-5 12 | velocity_error_prior_std: 1.0e-5 13 | rotation_error_prior_std: 1.0e-5 14 | gyro_bias_error_prior_std: 1.0e-5 15 | accelerometer_bias_error_prior_std: 1.0e-5 16 | 17 | # imu sensor noise 18 | gyro_noise_std: 1.0e-2 19 | accelerometer_noise_std: 1.0e-1 20 | 21 | # kalman measurement process std (uint: m) 22 | gps_position_x_std: 5 23 | gps_position_y_std: 5 24 | gps_position_z_std: 8 25 | 26 | # only use prediction, equivalent to only using IMU to integration 27 | only_prediction: false 28 | 29 | # use earth model 30 | use_earth_model: true 31 | -------------------------------------------------------------------------------- /data/display_path.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from mpl_toolkits import mplot3d 4 | 5 | 6 | def load_txt_data(data_path): 7 | try: 8 | return np.loadtxt(data_path) 9 | except FileNotFoundError as err: 10 | print('this is a OSError: ' + str(err)) 11 | 12 | 13 | if __name__ == "__main__": 14 | fuse_data_path = './fused.txt' 15 | gps_data_path = './gps_measurement.txt' 16 | gt_data_path = './gt.txt' 17 | 18 | fuse_data = load_txt_data(fuse_data_path) 19 | gps_data = load_txt_data(gps_data_path) 20 | gt_data = load_txt_data(gt_data_path) 21 | 22 | fig = plt.figure() 23 | ax = plt.axes(projection='3d') 24 | ax.set_title('compare path') 25 | ax.plot3D(fuse_data[:, 1], fuse_data[:, 2], fuse_data[:, 3], color='r', label='fuse_gps_imu') 26 | ax.plot3D(gps_data[:, 1], gps_data[:, 2], gps_data[:, 3], color='g', alpha=0.5, label='gps') 27 | ax.plot3D(gt_data[:, 1], gt_data[:, 2], gt_data[:, 3], color='b', label='ground_truth') 28 | plt.legend(loc='best') 29 | plt.show() 30 | -------------------------------------------------------------------------------- /data/images/0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zm0612/eskf-gps-imu-fusion/4b57efe96dbf540fe4b485ee54309ee35fb91d29/data/images/0.png -------------------------------------------------------------------------------- /data/images/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zm0612/eskf-gps-imu-fusion/4b57efe96dbf540fe4b485ee54309ee35fb91d29/data/images/1.png -------------------------------------------------------------------------------- /data/images/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zm0612/eskf-gps-imu-fusion/4b57efe96dbf540fe4b485ee54309ee35fb91d29/data/images/2.png -------------------------------------------------------------------------------- /data/images/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zm0612/eskf-gps-imu-fusion/4b57efe96dbf540fe4b485ee54309ee35fb91d29/data/images/3.png -------------------------------------------------------------------------------- /gnss-inss-sim/my_test.csv: -------------------------------------------------------------------------------- 1 | ini lat (deg),ini lon (deg),ini alt (m),ini vx_body (m/s),ini vy_body (m/s),ini vz_body (m/s),ini yaw (deg),ini pitch (deg),ini roll (deg) 2 | 32,120,0,10,0,0,0,0,0 3 | command type,yaw (deg),pitch (deg),roll (deg),vx_body (m/s),vy_body (m/s),vz_body (m/s),command duration (s),GPS visibility 4 | 1,9,0,0,0,0,0,10,1 5 | 1,0,0,0,0,0,0,10,1 6 | 1,-9,0,0,0,0,0,10,1 7 | -------------------------------------------------------------------------------- /gnss-inss-sim/my_test.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Filename: demo_no_algo.py 3 | 4 | """ 5 | The simplest demo of Sim. 6 | Only generate reference trajectory (pos, vel, sensor output). No algorithm. 7 | Created on 2018-01-23 8 | @author: dongxiaoguang 9 | """ 10 | 11 | import os 12 | import math 13 | from gnss_ins_sim.sim import imu_model 14 | from gnss_ins_sim.sim import ins_sim 15 | 16 | # globals 17 | D2R = math.pi/180 18 | 19 | motion_def_path = os.path.abspath('.//demo_motion_def_files//') 20 | fs = 100.0 # IMU sample frequency 21 | fs_gps = 10.0 # GPS sample frequency 22 | fs_mag = fs # magnetometer sample frequency, not used for now 23 | 24 | def test_path_gen(): 25 | ''' 26 | test only path generation in Sim. 27 | ''' 28 | #### choose a built-in IMU model, typical for IMU381 29 | imu_err = 'mid-accuracy' 30 | # generate GPS and magnetometer data 31 | imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True) 32 | 33 | #### start simulation 34 | sim = ins_sim.Sim([fs, fs_gps, fs_mag], 35 | motion_def_path+"//my_test.csv", 36 | ref_frame=0, 37 | imu=imu, 38 | mode=None, 39 | env=None, 40 | algorithm=None) 41 | sim.run(1) 42 | # save simulation data to files 43 | sim.results('') 44 | # plot data, 3d plot of reference positoin, 2d plots of gyro and accel 45 | sim.plot(['ref_pos', 'gyro', "ref_vel"], opt={'ref_pos': '3d'}) 46 | 47 | if __name__ == '__main__': 48 | test_path_gen() 49 | -------------------------------------------------------------------------------- /gnss-inss-sim/readme.md: -------------------------------------------------------------------------------- 1 | 这里只提供我所使用的[gnss-ins-sim](https://github.com/Aceinna/gnss-ins-sim)文件和运动参数文件! 2 | 3 | > `gnss-ins-sim`是一个用于仿真imu、gps、磁力计数据的软件 4 | 5 | ## 1.准备工作 6 | 7 | a. 如果想要运行`my_test.py`，你需要下载完整的[gnss-ins-sim](https://github.com/Aceinna/gnss-ins-sim) 8 | 9 | ```bash 10 | git clone https://github.com/Aceinna/gnss-ins-sim.git 11 | ``` 12 | 13 | b. 下载下来之后，将`my_test.csv`拷贝到`gnss-ins-sim/demo_motion_def_files/` 14 | 15 | ```bash 16 | cp my_test.csv gnss-ins-sim/demo_motion_def_files/ 17 | ``` 18 | 19 | c. 将`my_test.py`拷贝到`gnss-ins-sim/` 20 | 21 | ```bash 22 | cp my_test.py gnss-ins-sim 23 | ``` 24 | 25 | ## 2.运行 26 | 27 | ```bash 28 | cd gnss-ins-sim 29 | python my_test.py 30 | ``` 31 | 32 | > 执行完上一步命令之后，会在`gnss-ins-sim/demo_saved_data`文件夹下生成一个以时间信息为名称的文件夹，里面的数据包含了imu、gps所需要的所有数据 33 | 34 | ## 3.注意 35 | 36 | - [gnss-ins-sim](https://github.com/Aceinna/gnss-ins-sim)是一个用于仿真imu、gps、磁力计数据的软件，是一个非常不错的小工具，使用起来非常容易，建议你看一下它的[README介绍文件](https://github.com/Aceinna/gnss-ins-sim#gnss-ins-sim)。 37 | - 一定要注意坐标系之间的关系，imu是body frame(前后下)，gps位置是lla形式，导航系是在NED坐标系，gps数据通过`GeographicLib`库转换之后对应的是ENU(东北天)坐标系，我在代码中将其转到了NED，这之间的坐标关系，你一定要理清楚，不然很容易就会出错。 38 | 39 | -------------------------------------------------------------------------------- /include/common_tool.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/2/26. 3 | // 4 | 5 | #ifndef GPS_IMU_FUSION_COMMON_TOOL_H 6 | #define GPS_IMU_FUSION_COMMON_TOOL_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #define kDegree2Radian (M_PI / 180.0) 13 | 14 | inline void TransformCoordinate(Eigen::Vector3d &vec) { 15 | Eigen::Quaterniond Q_b_w = Eigen::AngleAxisd(90 * kDegree2Radian, Eigen::Vector3d::UnitZ()) * 16 | Eigen::AngleAxisd(0 * kDegree2Radian, Eigen::Vector3d::UnitY()) * 17 | Eigen::AngleAxisd(180 * kDegree2Radian, Eigen::Vector3d::UnitX()); 18 | 19 | vec = Q_b_w.inverse() * vec; 20 | } 21 | 22 | template 23 | inline Eigen::Matrix 24 | NedToEnu(const Eigen::Matrix &data) { 25 | Eigen::AngleAxis angle_axisd_x(static_cast(M_PI), Eigen::Matrix::UnitX()); 26 | Eigen::AngleAxis angle_axisd_z(static_cast(M_PI_2), Eigen::Matrix::UnitZ()); 27 | 28 | Eigen::Matrix R = angle_axisd_z.toRotationMatrix() * angle_axisd_x.toRotationMatrix(); 29 | 30 | return R * data; 31 | } 32 | 33 | template 34 | inline Derived NedToEnu(const Eigen::QuaternionBase &q) { 35 | Eigen::AngleAxis angle_axisd_x(static_cast(M_PI), 36 | Eigen::Matrix::UnitX()); 37 | 38 | Eigen::AngleAxis angle_axisd_z(static_cast(M_PI_2), 39 | Eigen::Matrix::UnitZ()); 40 | Eigen::Matrix R = 41 | angle_axisd_z.toRotationMatrix() * angle_axisd_x.toRotationMatrix(); 42 | 43 | Derived q_trans = Derived(R) * q; 44 | 45 | return q_trans; 46 | } 47 | 48 | //四元数 --> 欧拉角(Z-Y-X，即RPY)（确保pitch的范围[-pi/2, pi/2]） 49 | inline Eigen::Vector3d Quaternion2EulerAngles(Eigen::Quaterniond q) { 50 | Eigen::Vector3d angles; 51 | 52 | // roll (x-axis rotation) 53 | double sinr_cosp = 2 * (q.w() * q.x() + q.y() * q.z()); 54 | double cosr_cosp = 1 - 2 * (q.x() * q.x() + q.y() * q.y()); 55 | angles(2) = std::atan2(sinr_cosp, cosr_cosp); 56 | 57 | // pitch (y-axis rotation) 58 | double sinp = 2 * (q.w() * q.y() - q.z() * q.x()); 59 | if (std::abs(sinp) >= 1) 60 | angles(1) = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range 61 | else 62 | angles(1) = std::asin(sinp); 63 | 64 | // yaw (z-axis rotation) 65 | double siny_cosp = 2 * (q.w() * q.z() + q.x() * q.y()); 66 | double cosy_cosp = 1 - 2 * (q.y() * q.y() + q.z() * q.z()); 67 | angles(0) = std::atan2(siny_cosp, cosy_cosp); 68 | 69 | return angles; 70 | } 71 | 72 | inline Eigen::Vector3d Rotation2EulerAngles(Eigen::Matrix3d R) { 73 | //旋转矩阵 --> 欧拉角(Z-Y-X，即RPY)（确保pitch的范围[-pi/2, pi/2]） 74 | Eigen::Vector3d eulerAngle_mine; 75 | Eigen::Matrix3d rot = std::move(R); 76 | eulerAngle_mine(2) = std::atan2(rot(2, 1), rot(2, 2)); 77 | eulerAngle_mine(1) = std::atan2(-rot(2, 0), std::sqrt(rot(2, 1) * rot(2, 1) + rot(2, 2) * rot(2, 2))); 78 | eulerAngle_mine(0) = std::atan2(rot(1, 0), rot(0, 0)); 79 | 80 | return eulerAngle_mine; 81 | } 82 | 83 | template 84 | Eigen::Matrix BuildSkewSymmetricMatrix(const Eigen::MatrixBase &vec) { 85 | Eigen::Matrix matrix; 86 | matrix << static_cast(0.0), -vec[2], vec[1], 87 | vec[2], static_cast(0.0), -vec[0], 88 | -vec[1], vec[0], static_cast(0.0); 89 | 90 | return matrix; 91 | } 92 | 93 | template 94 | inline Eigen::Matrix SO3Exp(const Eigen::MatrixBase &v) { 95 | Eigen::Matrix R; 96 | typename Derived::Scalar theta = v.norm(); 97 | Eigen::Matrix v_normalized = v.normalized(); 98 | R = std::cos(theta) * Eigen::Matrix::Identity() 99 | + (typename Derived::Scalar(1.0) - std::cos(theta)) * v_normalized * 100 | v_normalized.transpose() + std::sin(theta) * BuildSkewSymmetricMatrix(v_normalized); 101 | 102 | return R; 103 | } 104 | 105 | #endif //GPS_IMU_FUSION_COMMON_TOOL_H 106 | -------------------------------------------------------------------------------- /include/config_parameters.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Zhang Zhimeng on 23-8-16. 3 | // 4 | 5 | #ifndef GPS_IMU_FUSION_CONFIG_PARAMETERS_H 6 | #define GPS_IMU_FUSION_CONFIG_PARAMETERS_H 7 | 8 | #include 9 | #include 10 | 11 | class ConfigParameters { 12 | public: 13 | ConfigParameters() = default; 14 | 15 | void LoadParameters(const std::string &config_file_path) { 16 | try { 17 | YAML::Node config_node = YAML::LoadFile(config_file_path); 18 | 19 | earth_rotation_speed_ = config_node["earth_rotation_speed"].as(); 20 | earth_gravity_ = config_node["earth_gravity"].as(); 21 | ref_longitude_ = config_node["ref_longitude"].as(); 22 | ref_latitude_ = config_node["ref_latitude"].as(); 23 | ref_altitude_ = config_node["ref_altitude"].as(); 24 | position_error_prior_std_ = config_node["position_error_prior_std"].as(); 25 | velocity_error_prior_std_ = config_node["velocity_error_prior_std"].as(); 26 | rotation_error_prior_std_ = config_node["rotation_error_prior_std"].as(); 27 | accelerometer_bias_error_prior_std_ = config_node["accelerometer_bias_error_prior_std"].as(); 28 | gyro_bias_error_prior_std_ = config_node["gyro_bias_error_prior_std"].as(); 29 | gyro_noise_std_ = config_node["gyro_noise_std"].as(); 30 | accelerometer_noise_std_ = config_node["accelerometer_noise_std"].as(); 31 | gps_position_x_std_ = config_node["gps_position_x_std"].as(); 32 | gps_position_y_std_ = config_node["gps_position_y_std"].as(); 33 | gps_position_z_std_ = config_node["gps_position_z_std"].as(); 34 | only_prediction_ = config_node["only_prediction"].as(); 35 | use_earth_model_ = config_node["use_earth_model"].as(); 36 | 37 | LOG(INFO) << "####### Config Parameters #######"; 38 | LOG(INFO) << "earth_rotation_speed: " << earth_rotation_speed_; 39 | LOG(INFO) << "earth_gravity: " << earth_gravity_; 40 | LOG(INFO) << "ref_longitude: " << ref_longitude_; 41 | LOG(INFO) << "ref_latitude: " << ref_latitude_; 42 | LOG(INFO) << "ref_altitude: " << ref_altitude_; 43 | LOG(INFO) << "position_error_prior_std: " << position_error_prior_std_; 44 | LOG(INFO) << "velocity_error_prior_std: " << velocity_error_prior_std_; 45 | LOG(INFO) << "rotation_error_prior_std: " << rotation_error_prior_std_; 46 | LOG(INFO) << "accelerometer_bias_error_prior_std: " << accelerometer_bias_error_prior_std_; 47 | LOG(INFO) << "gyro_bias_error_prior_std: " << gyro_bias_error_prior_std_; 48 | LOG(INFO) << "gyro_noise_std: " << gyro_noise_std_; 49 | LOG(INFO) << "accelerometer_noise_std: " << accelerometer_noise_std_; 50 | LOG(INFO) << "gps_position_x_std: " << gps_position_x_std_; 51 | LOG(INFO) << "gps_position_y_std: " << gps_position_y_std_; 52 | LOG(INFO) << "gps_position_z_std: " << gps_position_z_std_; 53 | LOG(INFO) << "only_prediction: " << only_prediction_; 54 | LOG(INFO) << "use_earth_model: " << use_earth_model_; 55 | LOG(INFO) << std::endl; 56 | } catch (...) { 57 | LOG(FATAL) << "Load config parameters failure, path: " << config_file_path; 58 | } 59 | } 60 | 61 | public: 62 | // earth 63 | double earth_rotation_speed_{}; 64 | double earth_gravity_{}; 65 | 66 | // reference point 67 | double ref_longitude_{}; 68 | double ref_latitude_{}; 69 | double ref_altitude_{}; 70 | 71 | // kalman prediction process std 72 | double position_error_prior_std_{}; 73 | double velocity_error_prior_std_{}; 74 | double rotation_error_prior_std_{}; 75 | double accelerometer_bias_error_prior_std_{}; 76 | double gyro_bias_error_prior_std_{}; 77 | 78 | // imu sensor noise 79 | double gyro_noise_std_{}; 80 | double accelerometer_noise_std_{}; 81 | 82 | // kalman measurement process std 83 | double gps_position_x_std_{}; 84 | double gps_position_y_std_{}; 85 | double gps_position_z_std_{}; 86 | 87 | // only using IMU to integration 88 | bool only_prediction_{}; 89 | 90 | bool use_earth_model_{}; 91 | }; 92 | 93 | #endif //GPS_IMU_FUSION_CONFIG_PARAMETERS_H 94 | -------------------------------------------------------------------------------- /include/eskf.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/2/19. 3 | // 4 | 5 | #ifndef GPS_IMU_FUSION_ESKF_H 6 | #define GPS_IMU_FUSION_ESKF_H 7 | 8 | #include "imu_data.h" 9 | #include "gps_data.h" 10 | #include "config_parameters.h" 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | class ErrorStateKalmanFilter { 18 | public: 19 | explicit ErrorStateKalmanFilter(const ConfigParameters &config_parameters); 20 | 21 | /*! 22 | * 用于ESKF滤波器的初始化，设置初始位姿，初始速度 23 | * @param curr_gps_data 与imu时间同步的gps数据 24 | * @param curr_imu_data 与gps时间同步的imu数据 25 | * @return 26 | */ 27 | bool Init(const GPSData &curr_gps_data, const IMUData &curr_imu_data); 28 | 29 | /*! 30 | * IMU姿态解算，以及滤波器的预测步的构建，对应卡尔曼滤波器的前两个公式 31 | * @param curr_imu_data 32 | * @return 33 | */ 34 | bool Predict(const IMUData &curr_imu_data); 35 | 36 | /*! 37 | * 滤波器的矫正，对应卡尔曼滤波器的后三个公式 38 | * @param curr_gps_data 39 | * @return 40 | */ 41 | bool Correct(const GPSData &curr_gps_data); 42 | 43 | Eigen::Matrix4d GetPose() const; 44 | 45 | Eigen::Vector3d GetVelocity() { 46 | return velocity_; 47 | } 48 | 49 | private: 50 | void SetCovarianceQ(double gyro_noise_cov, double accel_noise_cov); 51 | 52 | /*! 53 | * 54 | * @param position_x_std 55 | */ 56 | void SetCovarianceR(double position_x_std, double position_y_std, double position_z_std); 57 | 58 | void SetCovarianceP(double posi_noise, double velocity_noise, double ori_noise, 59 | double gyro_noise, double accel_noise); 60 | 61 | /*! 62 | * 通过IMU计算位姿和速度 63 | * @return 64 | */ 65 | void UpdateOdomEstimation(const Eigen::Vector3d &w_in); 66 | 67 | void UpdateErrorState(double t, const Eigen::Vector3d &accel, const Eigen::Vector3d &w_in_n); 68 | 69 | Eigen::Vector3d ComputeDeltaRotation(const IMUData &imu_data_0, const IMUData &imu_data_1); 70 | 71 | /*! 72 | * 计算地球转动给导航系带来的变换 73 | * note: 对于普通消费级IMU可以忽略此项 74 | * @param R_nm_nm_1 75 | * @return 76 | */ 77 | Eigen::Vector3d ComputeNavigationFrameAngularVelocity(); 78 | 79 | /*! 80 | * 通过IMU计算当前姿态 81 | * @param angular_delta 82 | * @param R_nm_nm_1 83 | * @param curr_R 84 | * @param last_R 85 | * @return 86 | */ 87 | void ComputeOrientation(const Eigen::Vector3d &angular_delta, 88 | const Eigen::Matrix3d &R_nm_nm_1, 89 | Eigen::Matrix3d &curr_R, 90 | Eigen::Matrix3d &last_R); 91 | 92 | void ComputeVelocity(const Eigen::Matrix3d &R_0, const Eigen::Matrix3d &R_1, const IMUData &imu_data_0, 93 | const IMUData &imu_data_1, Eigen::Vector3d &last_vel, Eigen::Vector3d &curr_vel); 94 | 95 | Eigen::Vector3d ComputeUnbiasAccel(const Eigen::Vector3d &accel); 96 | 97 | Eigen::Vector3d ComputeUnbiasGyro(const Eigen::Vector3d &gyro); 98 | 99 | /*! 100 | * 通过imu计算当前位移 101 | * @param curr_vel 102 | * @param last_vel 103 | * @return 104 | */ 105 | void ComputePosition(const Eigen::Matrix3d &R_0, const Eigen::Matrix3d &R_1, 106 | const Eigen::Vector3d &last_vel, const Eigen::Vector3d &curr_vel, 107 | const IMUData &imu_data_0, const IMUData &imu_data_1); 108 | 109 | /*! 110 | * 对误差进行滤波之后，需要在实际算出来的轨迹中，消除这部分误差 111 | */ 112 | void EliminateError(); 113 | 114 | /*! 115 | * 每次矫正之后，需要重置状态变量X 116 | */ 117 | void ResetState(); 118 | 119 | private: 120 | static const unsigned int DIM_STATE = 15; 121 | static const unsigned int DIM_STATE_NOISE = 6; 122 | static const unsigned int DIM_MEASUREMENT = 3; 123 | static const unsigned int DIM_MEASUREMENT_NOISE = 3; 124 | 125 | static const unsigned int INDEX_STATE_POSI = 0; 126 | static const unsigned int INDEX_STATE_VEL = 3; 127 | static const unsigned int INDEX_STATE_ORI = 6; 128 | static const unsigned int INDEX_STATE_GYRO_BIAS = 9; 129 | static const unsigned int INDEX_STATE_ACC_BIAS = 12; 130 | static const unsigned int INDEX_MEASUREMENT_POSI = 0; 131 | 132 | typedef typename Eigen::Matrix TypeVectorX; 133 | typedef typename Eigen::Matrix TypeVectorY; 134 | typedef typename Eigen::Matrix TypeMatrixF; 135 | typedef typename Eigen::Matrix TypeMatrixB; 136 | typedef typename Eigen::Matrix TypeMatrixQ; 137 | typedef typename Eigen::Matrix TypeMatrixP; 138 | typedef typename Eigen::Matrix TypeMatrixK; 139 | typedef typename Eigen::Matrix TypeMatrixC; 140 | typedef typename Eigen::Matrix TypeMatrixG; 141 | typedef typename Eigen::Matrix TypeMatrixR; 142 | 143 | TypeVectorX X_ = TypeVectorX::Zero(); 144 | TypeVectorY Y_ = TypeVectorY::Zero(); 145 | TypeMatrixF F_ = TypeMatrixF::Zero(); 146 | TypeMatrixB B_ = TypeMatrixB::Zero(); 147 | TypeMatrixQ Q_ = TypeMatrixQ::Zero(); 148 | TypeMatrixP P_ = TypeMatrixP::Zero(); 149 | TypeMatrixK K_ = TypeMatrixK::Zero(); 150 | TypeMatrixC C_ = TypeMatrixC::Zero(); 151 | TypeMatrixG G_ = TypeMatrixG::Zero(); 152 | TypeMatrixC R_ = TypeMatrixR::Zero(); 153 | 154 | TypeMatrixF Ft_ = TypeMatrixF::Zero(); 155 | 156 | Eigen::Vector3d velocity_ = Eigen::Vector3d::Zero(); 157 | Eigen::Matrix4d pose_ = Eigen::Matrix4d::Identity(); 158 | 159 | Eigen::Vector3d gyro_bias_ = Eigen::Vector3d::Zero(); 160 | Eigen::Vector3d accel_bias_ = Eigen::Vector3d::Zero(); 161 | 162 | Eigen::Vector3d g_ = Eigen::Vector3d::Zero();//重力加速度 163 | 164 | GPSData curr_gps_data_; 165 | 166 | double earth_rotation_speed_{0.0}; 167 | 168 | std::deque imu_data_buff_; 169 | 170 | ConfigParameters config_parameters_; 171 | public: 172 | void GetFGY(TypeMatrixF &F, TypeMatrixG &G, TypeVectorY &Y); 173 | }; 174 | 175 | #endif //GPS_IMU_FUSION_ESKF_H -------------------------------------------------------------------------------- /include/eskf_flow.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/2/24. 3 | // 4 | #ifndef GPS_IMU_FUSION_ESKF_FLOW_H 5 | #define GPS_IMU_FUSION_ESKF_FLOW_H 6 | 7 | #include "eskf.h" 8 | #include "imu_tool.h" 9 | #include "gps_tool.h" 10 | #include "config_parameters.h" 11 | #include "observability_analysis.h" 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | class ESKFFlow { 18 | public: 19 | ESKFFlow() = delete; 20 | 21 | explicit ESKFFlow(const std::string &config_file_path, std::string data_file_path); 22 | 23 | /*! 24 | * 从本地文件中读取IMU和GPS的数据 25 | * @return 26 | */ 27 | void ReadData(); 28 | 29 | /*! 30 | * 对IMU和GPS数据进行时间戳对齐，该函数只在ESKF初始化时使用 31 | * @return 32 | */ 33 | bool ValidGPSAndIMUData(); 34 | 35 | bool Run(); 36 | 37 | bool TestRun(); 38 | 39 | /*! 40 | * 保存位姿，为kitti格式 41 | * @param ofs 42 | * @param pose 43 | */ 44 | static void SavePose(std::ofstream &ofs, const Eigen::Matrix4d &pose); 45 | 46 | /*! 47 | * Save TUM pose 48 | * 49 | * note: 50 | * timestamp x y z q_x q_y q_z q_w 51 | * 52 | * @param ofs 53 | * @param pose 54 | */ 55 | static void SaveTUMPose(std::ofstream &ofs, const Eigen::Quaterniond &q, 56 | const Eigen::Vector3d &t, double timestamp); 57 | 58 | private: 59 | ConfigParameters config_parameters_; 60 | 61 | std::shared_ptr eskf_ptr_; 62 | std::shared_ptr imu_flow_ptr_; 63 | std::shared_ptr gps_flow_ptr_; 64 | 65 | ObservabilityAnalysis observability_analysis;//可观测度分析工具 66 | 67 | std::deque imu_data_buff_; 68 | std::deque gps_data_buff_; 69 | 70 | IMUData curr_imu_data_; 71 | GPSData curr_gps_data_; 72 | 73 | bool use_observability_analysis_ = false;//是否进行可观测度分析 74 | 75 | const std::string config_file_path_; 76 | const std::string data_file_path_; 77 | }; 78 | 79 | #endif //GPS_IMU_FUSION_ESKF_FLOW_H 80 | -------------------------------------------------------------------------------- /include/gps_data.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/2/19. 3 | // 4 | 5 | #ifndef GPS_IMU_FUSION_GPS_DATA_H 6 | #define GPS_IMU_FUSION_GPS_DATA_H 7 | 8 | #include "Geocentric/LocalCartesian.hpp" 9 | #include 10 | 11 | class GPSData { 12 | public: 13 | GPSData() = default; 14 | 15 | Eigen::Vector3d position_lla = Eigen::Vector3d::Zero(); // LLA unit: degree 16 | Eigen::Vector3d velocity = Eigen::Vector3d::Zero(); // NED 17 | Eigen::Vector3d local_position_ned = Eigen::Vector3d::Zero(); 18 | 19 | Eigen::Vector3d true_velocity = Eigen::Vector3d::Zero(); 20 | Eigen::Vector3d true_position_lla = Eigen::Vector3d::Zero(); 21 | double time = 0.0; 22 | }; 23 | 24 | #endif //GPS_IMU_FUSION_GPS_DATA_H 25 | -------------------------------------------------------------------------------- /include/gps_tool.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/2/19. 3 | // 4 | 5 | #ifndef GPS_IMU_FUSION_GPS_TOOL_H 6 | #define GPS_IMU_FUSION_GPS_TOOL_H 7 | 8 | #include "gps_data.h" 9 | #include "Geocentric/LocalCartesian.hpp" 10 | 11 | #include 12 | #include 13 | 14 | class GPSTool { 15 | public: 16 | GPSTool() = delete; 17 | 18 | GPSTool(double lon, double lat, double altitude); 19 | 20 | void LLAToLocalNED(GPSData &gps_data); 21 | 22 | Eigen::Vector3d LLAToLocalNED(const Eigen::Vector3d &lla); 23 | 24 | void ReadGPSData(const std::string &path, std::vector &gps_data_vec, int skip_rows = 1); 25 | 26 | void ReadGPSData(const std::string &path, std::deque &gps_data_vec, int skip_rows = 1); 27 | 28 | private: 29 | GeographicLib::LocalCartesian geo_converter_; // only support ENU 30 | }; 31 | 32 | #endif //GPS_IMU_FUSION_GPS_TOOL_H 33 | -------------------------------------------------------------------------------- /include/imu_data.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/2/19. 3 | // 4 | 5 | #ifndef GPS_IMU_FUSION_IMU_DATA_H 6 | #define GPS_IMU_FUSION_IMU_DATA_H 7 | 8 | #include 9 | #include 10 | 11 | class IMUData { 12 | public: 13 | IMUData() = default; 14 | 15 | double time = 0.0; 16 | Eigen::Vector3d linear_accel = Eigen::Vector3d::Zero(); 17 | Eigen::Vector3d angle_velocity = Eigen::Vector3d::Zero(); 18 | 19 | Eigen::Vector3d true_linear_accel = Eigen::Vector3d::Zero(); 20 | Eigen::Vector3d true_angle_velocity = Eigen::Vector3d::Zero(); 21 | 22 | Eigen::Quaterniond true_q_enu = Eigen::Quaterniond::Identity(); 23 | Eigen::Vector3d true_t_enu = Eigen::Vector3d::Zero(); 24 | }; 25 | 26 | #endif //GPS_IMU_FUSION_IMU_DATA_H 27 | -------------------------------------------------------------------------------- /include/imu_tool.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/2/19. 3 | // 4 | 5 | #ifndef GPS_IMU_FUSION_IMU_TOOL_H 6 | #define GPS_IMU_FUSION_IMU_TOOL_H 7 | 8 | #include "imu_data.h" 9 | 10 | #include 11 | #include 12 | 13 | class IMUTool { 14 | public: 15 | IMUTool() = default; 16 | 17 | static void ReadIMUData(const std::string &path, 18 | std::vector &imu_data_buff, 19 | const int skip_rows = 1); 20 | 21 | static void ReadIMUData(const std::string &path, 22 | std::deque &imu_data_buff, 23 | int skip_rows = 1); 24 | }; 25 | 26 | #endif //GPS_IMU_FUSION_IMU_TOOL_H 27 | -------------------------------------------------------------------------------- /include/observability_analysis.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/3/1. 3 | // 4 | 5 | #ifndef GPS_IMU_FUSION_OBSERVABILITY_ANALYSIS_H 6 | #define GPS_IMU_FUSION_OBSERVABILITY_ANALYSIS_H 7 | 8 | #include 9 | 10 | #include 11 | 12 | class ObservabilityAnalysis{ 13 | public: 14 | typedef typename Eigen::Matrix TypeF; 15 | typedef typename Eigen::Matrix TypeG; 16 | typedef typename Eigen::Matrix TypeY; 17 | 18 | ObservabilityAnalysis() = default; 19 | 20 | bool ComputeSOM(const int &interval = 100, const int& number = 1); 21 | 22 | bool ComputeObservability(); 23 | 24 | bool SaveFG(const TypeF &F, const TypeG &G, const TypeY &Y, const double& time); 25 | 26 | private: 27 | struct FG 28 | { 29 | double time; 30 | Eigen::Matrix F; 31 | Eigen::Matrix G; 32 | std::vector> Y; 33 | }; 34 | 35 | std::vector FGs_; 36 | 37 | Eigen::MatrixXd Qso_; 38 | Eigen::MatrixXd Ys_; 39 | 40 | std::vector F_buffer_; 41 | std::vector G_buffer_; 42 | std::vector Y_buffer_; 43 | }; 44 | #endif //GPS_IMU_FUSION_OBSERVABILITY_ANALYSIS_H 45 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # 误差状态卡尔曼滤波器(ESKF)融合IMU与GPS数据 2 | 3 | | 只使用IMU进行积分的结果 | 使用ESKF融合IMU和GPS | 4 | | :----------------------------------------------------------: | :----------------------------------------------------------: | 5 | | 0 | 0 | 6 | | 0 | 0 | 7 | 8 | 实现方法请参考我的博客[《【附源码+代码注释】误差状态卡尔曼滤波(error-state Kalman Filter)实现GPS+IMU融合，EKF ErrorStateKalmanFilter GPS+IMU》](https://blog.csdn.net/u011341856/article/details/114262451) 9 | 10 | ## 1. 依赖库 11 | 12 | Eigen 13 | 14 | ```shell 15 | sudo apt-get install libeigen3-dev 16 | ``` 17 | 18 | Yaml 19 | 20 | ```shell 21 | sudo apt-get install libyaml-cpp-dev 22 | ``` 23 | 24 | Glog 25 | ```shell 26 | sudo apt-get install libgoogle-glog-dev 27 | ``` 28 | 29 | ## 2. 编译 30 | 31 | ```shell 32 | cd eskf-gps-imu-fusion 33 | mkdir build 34 | cd build 35 | cmake .. 36 | make 37 | ``` 38 | 39 | ## 3. 运行 40 | 41 | ```shell 42 | cd eskf-gps-imu-fusion 43 | ./build/gps_imu_fusion ./config/config.yaml ./data 44 | ``` 45 | 46 | ## 4.轨迹显示 47 | 48 | 执行完`./gps_imu_fusion`会生成轨迹文件 49 | ```shell 50 | cd eskf-gps-imu-fusion/data 51 | python display_path.py 52 | ``` 53 | 54 | ## 5.误差分析 55 | 56 | __推荐使用工具__: [evo](https://github.com/MichaelGrupp/evo) 57 | ```shell 58 | cd eskf-gps-imu-fusion/data 59 | evo_traj tum fused.txt gt.txt gps_measurement.txt -p 60 | ``` 61 | 62 | ## 6. 接入其他数据 63 | 如果需要接入其他数据，您需要将您的数据格式进行整理，以符合本算法的需求，参考`data/raw_data`文件夹中的数据格式，并且至少要在`accel-0.csv`,`gps-0.csv`,`gps_time.csv`,`gyro-0.csv`,`time.csv`文件中填入你的IMU和GPS数据。 64 | 65 | **提示：** 66 | - IMU的加速度和角速度需要使用前右下坐标系； 67 | - GPS的数据按照：纬度、经度、高度填入，并且单位分别为度和米； 68 | - 采集和生成自己的数据时，请尽量从静止和近水平面状态开始运动。 69 | 70 | ## 7. todo 71 | - [ ] 增加初始化时重力对齐 72 | - [ ] 增加初始化时bias估计 73 | - [ ] …… 74 | -------------------------------------------------------------------------------- /src/eskf.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/2/19. 3 | // 4 | #include "eskf.h" 5 | #include "gps_tool.h" 6 | #include "common_tool.h" 7 | 8 | ErrorStateKalmanFilter::ErrorStateKalmanFilter(const ConfigParameters &config_parameters) 9 | : config_parameters_(config_parameters) { 10 | 11 | earth_rotation_speed_ = config_parameters_.earth_rotation_speed_; 12 | g_ = Eigen::Vector3d(0.0, 0.0, -config_parameters_.earth_gravity_); 13 | 14 | SetCovarianceP(config_parameters_.position_error_prior_std_, 15 | config_parameters_.velocity_error_prior_std_, 16 | config_parameters_.rotation_error_prior_std_, 17 | config_parameters_.gyro_bias_error_prior_std_, 18 | config_parameters_.accelerometer_bias_error_prior_std_); 19 | 20 | SetCovarianceR(config_parameters_.gps_position_x_std_, 21 | config_parameters_.gps_position_y_std_, 22 | config_parameters_.gps_position_z_std_); 23 | 24 | SetCovarianceQ(config_parameters_.gyro_noise_std_, config_parameters_.accelerometer_noise_std_); 25 | 26 | X_.setZero(); 27 | F_.setZero(); 28 | C_.setIdentity(); 29 | G_.block<3, 3>(INDEX_MEASUREMENT_POSI, INDEX_MEASUREMENT_POSI) = Eigen::Matrix3d::Identity(); 30 | } 31 | 32 | void ErrorStateKalmanFilter::SetCovarianceQ(double gyro_noise, double accel_noise) { 33 | Q_.setZero(); 34 | Q_.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() * gyro_noise * gyro_noise; 35 | Q_.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() * accel_noise * accel_noise; 36 | } 37 | 38 | void ErrorStateKalmanFilter::SetCovarianceR(double position_x_std, double position_y_std, double position_z_std) { 39 | R_.setZero(); 40 | R_(0, 0) = position_x_std * position_x_std; 41 | R_(1, 1) = position_y_std * position_y_std; 42 | R_(2, 2) = position_z_std * position_z_std; 43 | } 44 | 45 | void ErrorStateKalmanFilter::SetCovarianceP(double posi_noise, double velocity_noise, double ori_noise, 46 | double gyro_noise, double accel_noise) { 47 | P_.setZero(); 48 | P_.block<3, 3>(INDEX_STATE_POSI, INDEX_STATE_POSI) = Eigen::Matrix3d::Identity() * posi_noise * posi_noise; 49 | P_.block<3, 3>(INDEX_STATE_VEL, INDEX_STATE_VEL) = Eigen::Matrix3d::Identity() * velocity_noise * velocity_noise; 50 | P_.block<3, 3>(INDEX_STATE_ORI, INDEX_STATE_ORI) = Eigen::Matrix3d::Identity() * ori_noise * ori_noise; 51 | P_.block<3, 3>(INDEX_STATE_GYRO_BIAS, INDEX_STATE_GYRO_BIAS) = 52 | Eigen::Matrix3d::Identity() * gyro_noise * gyro_noise; 53 | P_.block<3, 3>(INDEX_STATE_ACC_BIAS, INDEX_STATE_ACC_BIAS) = 54 | Eigen::Matrix3d::Identity() * accel_noise * accel_noise; 55 | } 56 | 57 | bool ErrorStateKalmanFilter::Init(const GPSData &curr_gps_data, const IMUData &curr_imu_data) { 58 | velocity_ = curr_gps_data.true_velocity; 59 | 60 | Eigen::Quaterniond Q_init = Eigen::AngleAxisd(0 * kDegree2Radian, Eigen::Vector3d::UnitZ()) * 61 | Eigen::AngleAxisd(0 * kDegree2Radian, Eigen::Vector3d::UnitY()) * 62 | Eigen::AngleAxisd(0 * kDegree2Radian, Eigen::Vector3d::UnitX()); 63 | 64 | pose_.block<3, 3>(0, 0) = Q_init.toRotationMatrix(); 65 | pose_.block<3, 1>(0, 3) = curr_gps_data.local_position_ned; 66 | 67 | imu_data_buff_.clear(); 68 | imu_data_buff_.push_back(curr_imu_data); 69 | 70 | curr_gps_data_ = curr_gps_data; 71 | 72 | return true; 73 | } 74 | 75 | void ErrorStateKalmanFilter::GetFGY(TypeMatrixF &F, TypeMatrixG &G, TypeVectorY &Y) { 76 | F = Ft_; 77 | G = G_; 78 | Y = Y_; 79 | } 80 | 81 | bool ErrorStateKalmanFilter::Correct(const GPSData &curr_gps_data) { 82 | curr_gps_data_ = curr_gps_data; 83 | 84 | Y_ = curr_gps_data.local_position_ned - pose_.block<3, 1>(0, 3); 85 | 86 | K_ = P_ * G_.transpose() * (G_ * P_ * G_.transpose() + C_ * R_ * C_.transpose()).inverse(); 87 | 88 | P_ = (TypeMatrixP::Identity() - K_ * G_) * P_; 89 | X_ = X_ + K_ * (Y_ - G_ * X_); 90 | 91 | EliminateError(); 92 | 93 | ResetState(); 94 | 95 | return true; 96 | } 97 | 98 | bool ErrorStateKalmanFilter::Predict(const IMUData &curr_imu_data) { 99 | imu_data_buff_.push_back(curr_imu_data); 100 | 101 | Eigen::Vector3d w_in = Eigen::Vector3d::Zero(); 102 | 103 | if (config_parameters_.use_earth_model_) { 104 | w_in = ComputeNavigationFrameAngularVelocity(); // 时刻 m-1 -> m 地球转动引起的导航系转动角速度 105 | } 106 | UpdateOdomEstimation(w_in); 107 | 108 | double delta_t = curr_imu_data.time - imu_data_buff_.front().time; 109 | 110 | Eigen::Vector3d curr_accel = pose_.block<3, 3>(0, 0) * curr_imu_data.linear_accel; 111 | 112 | UpdateErrorState(delta_t, curr_accel, w_in); 113 | 114 | imu_data_buff_.pop_front(); 115 | 116 | return true; 117 | } 118 | 119 | void ErrorStateKalmanFilter::UpdateErrorState(double t, const Eigen::Vector3d &accel, const Eigen::Vector3d &w_in_n) { 120 | Eigen::Matrix3d F_23 = BuildSkewSymmetricMatrix(accel); 121 | Eigen::Matrix3d F_33 = -BuildSkewSymmetricMatrix(w_in_n); 122 | 123 | F_.block<3, 3>(INDEX_STATE_POSI, INDEX_STATE_VEL) = Eigen::Matrix3d::Identity(); 124 | F_.block<3, 3>(INDEX_STATE_VEL, INDEX_STATE_ORI) = F_23; 125 | F_.block<3, 3>(INDEX_STATE_ORI, INDEX_STATE_ORI) = F_33; 126 | F_.block<3, 3>(INDEX_STATE_VEL, INDEX_STATE_ACC_BIAS) = pose_.block<3, 3>(0, 0); 127 | F_.block<3, 3>(INDEX_STATE_ORI, INDEX_STATE_GYRO_BIAS) = -pose_.block<3, 3>(0, 0); 128 | B_.block<3, 3>(INDEX_STATE_VEL, 3) = pose_.block<3, 3>(0, 0); 129 | B_.block<3, 3>(INDEX_STATE_ORI, 0) = -pose_.block<3, 3>(0, 0); 130 | 131 | TypeMatrixF Fk = TypeMatrixF::Identity() + F_ * t; 132 | TypeMatrixB Bk = B_ * t; 133 | 134 | // 用于可观测性分析 135 | Ft_ = F_ * t; 136 | 137 | X_ = Fk * X_; 138 | P_ = Fk * P_ * Fk.transpose() + Bk * Q_ * Bk.transpose(); 139 | } 140 | 141 | void ErrorStateKalmanFilter::UpdateOdomEstimation(const Eigen::Vector3d &w_in) { 142 | const auto &last_imu_data = imu_data_buff_.at(0); 143 | const auto &curr_imu_data = imu_data_buff_.at(1); 144 | const double delta_t = curr_imu_data.time - last_imu_data.time; 145 | 146 | Eigen::Vector3d delta_rotation = ComputeDeltaRotation(last_imu_data, curr_imu_data); 147 | 148 | const Eigen::Vector3d phi_in = w_in * delta_t; 149 | const Eigen::AngleAxisd angle_axisd(phi_in.norm(), phi_in.normalized()); 150 | const Eigen::Matrix3d R_nm_nm_1 = angle_axisd.toRotationMatrix().transpose(); 151 | 152 | Eigen::Matrix3d curr_R; // R_n_m m时刻的旋转 153 | Eigen::Matrix3d last_R; // C_n_m_1 m-1时刻的旋转 154 | ComputeOrientation(delta_rotation, R_nm_nm_1, curr_R, last_R); 155 | 156 | Eigen::Vector3d curr_vel; // 当前时刻导航系下的速度 157 | Eigen::Vector3d last_vel; // 上一时刻导航系下的速度 158 | ComputeVelocity(last_R, curr_R, last_imu_data, curr_imu_data, last_vel, curr_vel); 159 | 160 | ComputePosition(last_R, curr_R, last_vel, curr_vel, last_imu_data, curr_imu_data); 161 | } 162 | 163 | Eigen::Vector3d ErrorStateKalmanFilter::ComputeDeltaRotation(const IMUData &imu_data_0, const IMUData &imu_data_1) { 164 | const double delta_t = imu_data_1.time - imu_data_0.time; 165 | 166 | CHECK_GT(delta_t, 0.0) << "IMU timestamp error"; 167 | 168 | const Eigen::Vector3d &unbias_gyro_0 = ComputeUnbiasGyro(imu_data_0.angle_velocity); 169 | const Eigen::Vector3d &unbias_gyro_1 = ComputeUnbiasGyro(imu_data_1.angle_velocity); 170 | 171 | Eigen::Vector3d delta_theta = 0.5 * (unbias_gyro_0 + unbias_gyro_1) * delta_t; 172 | 173 | return delta_theta; 174 | } 175 | 176 | Eigen::Vector3d ErrorStateKalmanFilter::ComputeNavigationFrameAngularVelocity() { 177 | const double latitude = curr_gps_data_.position_lla.y() * kDegree2Radian; 178 | const double height = curr_gps_data_.position_lla.z(); 179 | 180 | constexpr double f = 1.0 / 298.257223563; // 椭球扁率 181 | 182 | constexpr double Re = 6378137.0; // 椭圆长半轴 183 | constexpr double Rp = (1.0 - f) * Re; // 椭圆短半轴 184 | const double e = std::sqrt(Re * Re - Rp * Rp) / Re; // 椭圆的偏心率 185 | 186 | const double Rn = Re / std::sqrt(1.0 - e * e * std::sin(latitude) * std::sin(latitude)); // 子午圈主曲率半径 187 | const double Rm = Re * (1.0 - e * e) 188 | / std::pow(1.0 - e * e * std::sin(latitude) * std::sin(latitude), 3.0 / 2.0); // 卯酉圈主曲率半径 189 | 190 | // 由于载体在地球表面运动造成的导航系姿态变化。在导航系下表示 191 | Eigen::Vector3d w_en_n; 192 | w_en_n << velocity_[1] / (Rm + height), -velocity_[0] / (Rn + height), 193 | -velocity_[1] / (Rn + height) * std::tan(latitude); 194 | 195 | Eigen::Vector3d w_ie_n; 196 | w_ie_n << earth_rotation_speed_ * std::cos(latitude), 0.0, -earth_rotation_speed_ * std::sin(latitude); 197 | 198 | Eigen::Vector3d w_in_n = w_en_n + w_ie_n; 199 | 200 | return w_in_n; 201 | } 202 | 203 | void ErrorStateKalmanFilter::ComputeOrientation(const Eigen::Vector3d &angular_delta, 204 | const Eigen::Matrix3d &R_nm_nm_1, 205 | Eigen::Matrix3d &curr_R, 206 | Eigen::Matrix3d &last_R) { 207 | Eigen::AngleAxisd angle_axisd(angular_delta.norm(), angular_delta.normalized()); 208 | 209 | last_R = pose_.block<3, 3>(0, 0); 210 | curr_R = R_nm_nm_1.transpose() * pose_.block<3, 3>(0, 0) * angle_axisd.toRotationMatrix(); 211 | pose_.block<3, 3>(0, 0) = curr_R; 212 | } 213 | 214 | void ErrorStateKalmanFilter::ComputeVelocity(const Eigen::Matrix3d &R_0, const Eigen::Matrix3d &R_1, 215 | const IMUData &imu_data_0, const IMUData &imu_data_1, 216 | Eigen::Vector3d &last_vel, Eigen::Vector3d &curr_vel) { 217 | double delta_t = imu_data_1.time - imu_data_0.time; 218 | 219 | CHECK_GT(delta_t, 0.0) << "IMU timestamp error"; 220 | 221 | Eigen::Vector3d unbias_accel_0 = R_0 * ComputeUnbiasAccel(imu_data_0.linear_accel) - g_; 222 | Eigen::Vector3d unbias_accel_1 = R_1 * ComputeUnbiasAccel(imu_data_1.linear_accel) - g_; 223 | 224 | last_vel = velocity_; 225 | 226 | // 中值积分 227 | velocity_ += delta_t * 0.5 * (unbias_accel_0 + unbias_accel_1); 228 | 229 | curr_vel = velocity_; 230 | } 231 | 232 | Eigen::Vector3d ErrorStateKalmanFilter::ComputeUnbiasAccel(const Eigen::Vector3d &accel) { 233 | return accel - accel_bias_; 234 | } 235 | 236 | Eigen::Vector3d ErrorStateKalmanFilter::ComputeUnbiasGyro(const Eigen::Vector3d &gyro) { 237 | return gyro - gyro_bias_; 238 | } 239 | 240 | void ErrorStateKalmanFilter::ComputePosition(const Eigen::Matrix3d &R_0, const Eigen::Matrix3d &R_1, 241 | const Eigen::Vector3d &last_vel, const Eigen::Vector3d &curr_vel, 242 | const IMUData &imu_data_0, const IMUData &imu_data_1) { 243 | Eigen::Vector3d unbias_accel_0 = R_0 * ComputeUnbiasAccel(imu_data_0.linear_accel) - g_; 244 | Eigen::Vector3d unbias_accel_1 = R_1 * ComputeUnbiasAccel(imu_data_1.linear_accel) - g_; 245 | 246 | double delta_t = imu_data_1.time - imu_data_0.time; 247 | 248 | pose_.block<3, 1>(0, 3) += 0.5 * delta_t * (curr_vel + last_vel) + 249 | 0.25 * (unbias_accel_0 + unbias_accel_1) * delta_t * delta_t; 250 | } 251 | 252 | void ErrorStateKalmanFilter::ResetState() { 253 | X_.setZero(); 254 | } 255 | 256 | void ErrorStateKalmanFilter::EliminateError() { 257 | pose_.block<3, 1>(0, 3) = pose_.block<3, 1>(0, 3) + X_.block<3, 1>(INDEX_STATE_POSI, 0); 258 | 259 | velocity_ = velocity_ + X_.block<3, 1>(INDEX_STATE_VEL, 0); 260 | Eigen::Matrix3d C_nn = SO3Exp(-X_.block<3, 1>(INDEX_STATE_ORI, 0)); 261 | pose_.block<3, 3>(0, 0) = C_nn * pose_.block<3, 3>(0, 0); 262 | 263 | gyro_bias_ = gyro_bias_ + X_.block<3, 1>(INDEX_STATE_GYRO_BIAS, 0); 264 | accel_bias_ = accel_bias_ + X_.block<3, 1>(INDEX_STATE_ACC_BIAS, 0); 265 | } 266 | 267 | Eigen::Matrix4d ErrorStateKalmanFilter::GetPose() const { 268 | return pose_; 269 | } -------------------------------------------------------------------------------- /src/eskf_flow.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/2/24. 3 | // 4 | #include "config_parameters.h" 5 | #include "eskf_flow.h" 6 | #include "common_tool.h" 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | ESKFFlow::ESKFFlow(const std::string &config_file_path, std::string data_file_path) 14 | : config_file_path_(config_file_path), data_file_path_(std::move(data_file_path)) { 15 | config_parameters_.LoadParameters(config_file_path); 16 | 17 | gps_flow_ptr_ = std::make_shared(config_parameters_.ref_longitude_, 18 | config_parameters_.ref_latitude_, 19 | config_parameters_.ref_altitude_); 20 | eskf_ptr_ = std::make_shared(config_parameters_); 21 | } 22 | 23 | void ESKFFlow::ReadData() { 24 | imu_flow_ptr_->ReadIMUData(data_file_path_ + "/raw_data", imu_data_buff_); 25 | gps_flow_ptr_->ReadGPSData(data_file_path_ + "/raw_data", gps_data_buff_); 26 | } 27 | 28 | bool ESKFFlow::ValidGPSAndIMUData() { 29 | curr_imu_data_ = imu_data_buff_.front(); 30 | curr_gps_data_ = gps_data_buff_.front(); 31 | 32 | double delta_time = curr_imu_data_.time - curr_gps_data_.time; 33 | 34 | if (delta_time > 0.05) { 35 | gps_data_buff_.pop_front(); 36 | return false; 37 | } 38 | 39 | if (delta_time < -0.05) { 40 | imu_data_buff_.pop_front(); 41 | return false; 42 | } 43 | 44 | imu_data_buff_.pop_front(); 45 | gps_data_buff_.pop_front(); 46 | 47 | return true; 48 | } 49 | 50 | bool ESKFFlow::Run() { 51 | ReadData(); 52 | 53 | while (!imu_data_buff_.empty() && !gps_data_buff_.empty()) { 54 | if (!ValidGPSAndIMUData()) { 55 | continue; 56 | } else { 57 | eskf_ptr_->Init(curr_gps_data_, curr_imu_data_); 58 | break; 59 | } 60 | } 61 | 62 | std::ofstream gt_file(data_file_path_ + "/gt.txt", std::ios::trunc); 63 | std::ofstream fused_file(data_file_path_ + "/fused.txt", std::ios::trunc); 64 | std::ofstream measured_file(data_file_path_ + "/gps_measurement.txt", std::ios::trunc); 65 | 66 | LOG(INFO) << "Start fuse IMU and GPS ..."; 67 | while (!imu_data_buff_.empty() && !gps_data_buff_.empty()) { 68 | curr_imu_data_ = imu_data_buff_.front(); 69 | curr_gps_data_ = gps_data_buff_.front(); 70 | if (curr_imu_data_.time < curr_gps_data_.time) { 71 | eskf_ptr_->Predict(curr_imu_data_); 72 | imu_data_buff_.pop_front(); 73 | } else { 74 | eskf_ptr_->Predict(curr_imu_data_); 75 | imu_data_buff_.pop_front(); 76 | 77 | if (!config_parameters_.only_prediction_) { 78 | eskf_ptr_->Correct(curr_gps_data_); 79 | } 80 | 81 | SaveTUMPose(fused_file, Eigen::Quaterniond(eskf_ptr_->GetPose().topLeftCorner<3, 3>()), 82 | eskf_ptr_->GetPose().topRightCorner<3, 1>(), curr_imu_data_.time); 83 | 84 | SaveTUMPose(measured_file, Eigen::Quaterniond::Identity(), 85 | curr_gps_data_.local_position_ned, curr_gps_data_.time); 86 | 87 | SaveTUMPose(gt_file, curr_imu_data_.true_q_enu, 88 | gps_flow_ptr_->LLAToLocalNED(curr_imu_data_.true_t_enu), curr_imu_data_.time); 89 | 90 | gps_data_buff_.pop_front(); 91 | } 92 | 93 | if (use_observability_analysis_) { 94 | Eigen::Matrix F; 95 | Eigen::Matrix G; 96 | Eigen::Matrix Y; 97 | eskf_ptr_->GetFGY(F, G, Y); 98 | observability_analysis.SaveFG(F, G, Y, curr_gps_data_.time); 99 | } 100 | } 101 | 102 | if (use_observability_analysis_) { 103 | observability_analysis.ComputeSOM(); 104 | observability_analysis.ComputeObservability(); 105 | } 106 | 107 | LOG(INFO) << "End fuse IMU and GPS"; 108 | LOG(INFO) << "Ground Truth data in: " << data_file_path_ + "/gt.txt"; 109 | LOG(INFO) << "Fusion data in: " << data_file_path_ + "/fused.txt"; 110 | LOG(INFO) << "GPS data in: " << data_file_path_ + "/gps_measurement.txt"; 111 | 112 | return true; 113 | } 114 | 115 | bool ESKFFlow::TestRun() { 116 | ReadData(); 117 | 118 | while (!imu_data_buff_.empty() && !gps_data_buff_.empty()) { 119 | if (!ValidGPSAndIMUData()) { 120 | continue; 121 | } else { 122 | eskf_ptr_->Init(curr_gps_data_, curr_imu_data_); 123 | std::cout << "\ntime: " << curr_gps_data_.time << std::endl; 124 | std::cout << "vel: " << eskf_ptr_->GetVelocity().transpose() << std::endl; 125 | std::cout << "measure vel: " << curr_gps_data_.velocity.transpose() << std::endl; 126 | std::cout << "true vel: " << curr_gps_data_.true_velocity.transpose() << std::endl; 127 | std::cout << "time: " << curr_gps_data_.time << std::endl; 128 | break; 129 | } 130 | } 131 | 132 | std::ofstream gt_file(config_file_path_ + "/data/gt.txt", std::ios::trunc); 133 | std::ofstream fused_file(config_file_path_ + "/data/fused.txt", std::ios::trunc); 134 | std::ofstream measured_file(config_file_path_ + "/data/measured.txt", std::ios::trunc); 135 | 136 | while (!imu_data_buff_.empty() && !gps_data_buff_.empty()) { 137 | curr_imu_data_ = imu_data_buff_.front(); 138 | curr_gps_data_ = gps_data_buff_.front(); 139 | eskf_ptr_->Predict(curr_imu_data_); 140 | imu_data_buff_.pop_front(); 141 | SavePose(fused_file, eskf_ptr_->GetPose()); 142 | } 143 | 144 | return true; 145 | } 146 | 147 | void ESKFFlow::SavePose(std::ofstream &ofs, const Eigen::Matrix4d &pose) { 148 | for (int i = 0; i < 3; ++i) { 149 | for (int j = 0; j < 4; ++j) { 150 | ofs << pose(i, j); 151 | 152 | if (i == 2 && j == 3) { 153 | ofs << std::endl; 154 | } else { 155 | ofs << " "; 156 | } 157 | } 158 | } 159 | } 160 | 161 | void ESKFFlow::SaveTUMPose(std::ofstream &ofs, const Eigen::Quaterniond &q, 162 | const Eigen::Vector3d &t, double timestamp) { 163 | ofs << std::fixed << std::setprecision(10) << timestamp << " " << t.x() << " " << t.y() << " " << t.z() << " " 164 | << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl; 165 | } 166 | -------------------------------------------------------------------------------- /src/gps_tool.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/2/19. 3 | // 4 | #include "common_tool.h" 5 | #include "gps_tool.h" 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | GPSTool::GPSTool(double lon, double lat, double altitude) { 13 | GPSTool::geo_converter_.Reset(lat, lon, altitude); 14 | } 15 | 16 | Eigen::Vector3d GPSTool::LLAToLocalNED(const Eigen::Vector3d &lla) { 17 | Eigen::Vector3d enu; 18 | 19 | double enu_x, enu_y, enu_z; 20 | geo_converter_.Forward(lla.x(), lla.y(), lla.z(), 21 | enu_x, enu_y, enu_z); 22 | enu.x() = enu_y; 23 | enu.y() = enu_x; 24 | enu.z() = -enu_z; 25 | 26 | return enu; 27 | } 28 | 29 | void GPSTool::LLAToLocalNED(GPSData &gps_data) { 30 | // LLA -> ENU frame 31 | double enu_x, enu_y, enu_z; 32 | geo_converter_.Forward(gps_data.position_lla.x(), 33 | gps_data.position_lla.y(), 34 | gps_data.position_lla.z(), 35 | enu_x, enu_y, enu_z); 36 | 37 | // ENU -> NED 38 | gps_data.local_position_ned.x() = enu_y; 39 | gps_data.local_position_ned.y() = enu_x; 40 | gps_data.local_position_ned.z() = -enu_z; 41 | } 42 | 43 | void GPSTool::ReadGPSData(const std::string &path, std::vector &gps_data_vec, int skip_rows) { 44 | std::string gps_file_path = path + "/gps-0.csv"; 45 | std::string ref_gps_file_path = path + "/ref_gps.csv"; 46 | std::string time_file_path = path + "/gps_time.csv"; 47 | std::ifstream gps_file(gps_file_path, std::ios::in); 48 | std::ifstream ref_gps_file(ref_gps_file_path, std::ios::in); 49 | std::ifstream gps_time_file(time_file_path, std::ios::in); 50 | 51 | if (!gps_file.is_open() || !ref_gps_file.is_open() || !gps_time_file.is_open()) { 52 | LOG(FATAL) << "failure to open gps file"; 53 | } 54 | 55 | GPSData gps_data; 56 | gps_data_vec.clear(); 57 | 58 | std::string gps_data_line; 59 | std::string ref_gps_data_line; 60 | std::string gps_time_line; 61 | std::string temp; 62 | 63 | for (int i = 0; i < skip_rows; ++i) { 64 | std::getline(gps_file, temp); 65 | std::getline(ref_gps_file, temp); 66 | std::getline(gps_time_file, temp); 67 | } 68 | 69 | while (std::getline(gps_file, gps_data_line) 70 | && std::getline(ref_gps_file, ref_gps_data_line) 71 | && std::getline(gps_time_file, gps_time_line)) { 72 | gps_data.time = std::stod(gps_time_line); 73 | 74 | std::stringstream ssr_0; 75 | std::stringstream ssr_1; 76 | 77 | ssr_0 << gps_data_line; 78 | ssr_1 << ref_gps_data_line; 79 | 80 | std::getline(ssr_0, temp, ','); 81 | gps_data.position_lla.x() = std::stod(temp); 82 | 83 | std::getline(ssr_0, temp, ','); 84 | gps_data.position_lla.y() = std::stod(temp); 85 | 86 | std::getline(ssr_0, temp, ','); 87 | gps_data.position_lla.z() = std::stod(temp); 88 | 89 | std::getline(ssr_0, temp, ','); 90 | gps_data.velocity.x() = std::stod(temp); 91 | 92 | std::getline(ssr_0, temp, ','); 93 | gps_data.velocity.y() = std::stod(temp); 94 | 95 | std::getline(ssr_0, temp, ','); 96 | gps_data.velocity.z() = std::stod(temp); 97 | 98 | std::getline(ssr_1, temp, ','); 99 | gps_data.true_position_lla.x() = std::stod(temp); 100 | 101 | std::getline(ssr_1, temp, ','); 102 | gps_data.true_position_lla.y() = std::stod(temp); 103 | 104 | std::getline(ssr_1, temp, ','); 105 | gps_data.true_position_lla.z() = std::stod(temp); 106 | 107 | std::getline(ssr_1, temp, ','); 108 | gps_data.true_velocity.x() = std::stod(temp); 109 | 110 | std::getline(ssr_1, temp, ','); 111 | gps_data.true_velocity.y() = std::stod(temp); 112 | 113 | std::getline(ssr_1, temp, ','); 114 | gps_data.true_velocity.z() = std::stod(temp); 115 | 116 | LLAToLocalNED(gps_data); 117 | 118 | gps_data_vec.emplace_back(gps_data); 119 | } 120 | 121 | gps_time_file.close(); 122 | ref_gps_file.close(); 123 | ref_gps_file.close(); 124 | } 125 | 126 | void GPSTool::ReadGPSData(const std::string &path, std::deque &gps_data_vec, int skip_rows) { 127 | LOG(INFO) << "Read GPS data ..."; 128 | std::string gps_file_path = path + "/gps-0.csv"; 129 | std::string ref_gps_file_path = path + "/ref_gps.csv"; 130 | std::string time_file_path = path + "/gps_time.csv"; 131 | std::ifstream gps_file(gps_file_path, std::ios::in); 132 | std::ifstream ref_gps_file(ref_gps_file_path, std::ios::in); 133 | std::ifstream gps_time_file(time_file_path, std::ios::in); 134 | 135 | if (!gps_file.is_open() || !ref_gps_file.is_open() || !gps_time_file.is_open()) { 136 | LOG(FATAL) << "failure to open gps file"; 137 | } 138 | 139 | GPSData gps_data; 140 | gps_data_vec.clear(); 141 | 142 | std::string gps_data_line; 143 | std::string ref_gps_data_line; 144 | std::string gps_time_line; 145 | std::string temp; 146 | 147 | for (int i = 0; i < skip_rows; ++i) { 148 | std::getline(gps_file, temp); 149 | std::getline(ref_gps_file, temp); 150 | std::getline(gps_time_file, temp); 151 | } 152 | 153 | while (std::getline(gps_file, gps_data_line) 154 | && std::getline(ref_gps_file, ref_gps_data_line) 155 | && std::getline(gps_time_file, gps_time_line)) { 156 | gps_data.time = std::stod(gps_time_line); 157 | 158 | std::stringstream ssr_0; 159 | std::stringstream ssr_1; 160 | 161 | ssr_0 << gps_data_line; 162 | ssr_1 << ref_gps_data_line; 163 | 164 | std::getline(ssr_0, temp, ','); 165 | gps_data.position_lla.x() = std::stod(temp); 166 | 167 | std::getline(ssr_0, temp, ','); 168 | gps_data.position_lla.y() = std::stod(temp); 169 | 170 | std::getline(ssr_0, temp, ','); 171 | gps_data.position_lla.z() = std::stod(temp); 172 | 173 | std::getline(ssr_0, temp, ','); 174 | gps_data.velocity.x() = std::stod(temp); 175 | 176 | std::getline(ssr_0, temp, ','); 177 | gps_data.velocity.y() = std::stod(temp); 178 | 179 | std::getline(ssr_0, temp, ','); 180 | gps_data.velocity.z() = std::stod(temp); 181 | 182 | std::getline(ssr_1, temp, ','); 183 | gps_data.true_position_lla.x() = std::stod(temp); 184 | 185 | std::getline(ssr_1, temp, ','); 186 | gps_data.true_position_lla.y() = std::stod(temp); 187 | 188 | std::getline(ssr_1, temp, ','); 189 | gps_data.true_position_lla.z() = std::stod(temp); 190 | 191 | std::getline(ssr_1, temp, ','); 192 | gps_data.true_velocity.x() = std::stod(temp); 193 | 194 | std::getline(ssr_1, temp, ','); 195 | gps_data.true_velocity.y() = std::stod(temp); 196 | 197 | std::getline(ssr_1, temp, ','); 198 | gps_data.true_velocity.z() = std::stod(temp); 199 | 200 | LLAToLocalNED(gps_data); 201 | 202 | gps_data_vec.emplace_back(gps_data); 203 | } 204 | 205 | gps_time_file.close(); 206 | ref_gps_file.close(); 207 | ref_gps_file.close(); 208 | 209 | LOG(INFO) << "Read GPS data successfully"; 210 | } -------------------------------------------------------------------------------- /src/imu_tool.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/2/19. 3 | // 4 | #include "common_tool.h" 5 | #include "imu_tool.h" 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | void IMUTool::ReadIMUData(const std::string &path, std::vector &imu_data_buff, const int skip_rows) { 13 | LOG(INFO) << "Read IMU data ..."; 14 | std::string accel_file_path = path + "/accel-0.csv"; 15 | std::string ref_accel_file_path = path + "/ref_accel.csv"; 16 | std::string gyro_file_path = path + "/gyro-0.csv"; 17 | std::string ref_gyro_file_path = path + "/ref_gyro.csv"; 18 | std::string time_file_path = path + "/time.csv"; 19 | std::string ref_pos_file_path = path + "/ref_pos.csv"; 20 | std::string ref_att_quat_file_path = path + "/ref_att_quat.csv"; 21 | 22 | std::ifstream accel_file(accel_file_path); 23 | std::ifstream ref_accel_file(ref_accel_file_path); 24 | std::ifstream gyro_file(gyro_file_path); 25 | std::ifstream ref_gyro_file(ref_gyro_file_path); 26 | std::ifstream time_file(time_file_path); 27 | std::ifstream ref_att_quat_file(ref_att_quat_file_path); 28 | std::ifstream ref_pos_file(ref_pos_file_path); 29 | 30 | if (!accel_file.is_open() || !ref_accel_file.is_open() 31 | || !gyro_file.is_open() || !ref_gyro_file.is_open() 32 | || !time_file.is_open() || !ref_att_quat_file.is_open() 33 | || !ref_pos_file.is_open()) { 34 | LOG(FATAL) << "Failure to open IMU file"; 35 | } 36 | 37 | IMUData imu_data; 38 | imu_data_buff.clear(); 39 | 40 | std::string accel_line; 41 | std::string ref_accel_line; 42 | std::string gyro_line; 43 | std::string ref_gyro_line; 44 | std::string time_line; 45 | std::string ref_pos_line; 46 | std::string ref_att_quat_line; 47 | std::string temp; 48 | 49 | for (int i = 0; i < skip_rows; ++i) { 50 | std::getline(accel_file, temp); 51 | std::getline(ref_accel_file, temp); 52 | std::getline(gyro_file, temp); 53 | std::getline(ref_gyro_file, temp); 54 | std::getline(time_file, temp); 55 | std::getline(ref_pos_file, temp); 56 | std::getline(ref_att_quat_file, temp); 57 | } 58 | 59 | while (std::getline(accel_file, accel_line) && 60 | std::getline(ref_accel_file, ref_accel_line) && 61 | std::getline(gyro_file, gyro_line) && 62 | std::getline(ref_gyro_file, ref_gyro_line) && 63 | std::getline(time_file, time_line) && 64 | std::getline(ref_att_quat_file, ref_att_quat_line) && 65 | std::getline(ref_pos_file, ref_pos_line)) { 66 | imu_data.time = std::stod(time_line); 67 | 68 | std::stringstream ss; 69 | ss << accel_line; 70 | 71 | std::getline(ss, temp, ','); 72 | imu_data.linear_accel.x() = std::stod(temp); 73 | std::getline(ss, temp, ','); 74 | imu_data.linear_accel.y() = std::stod(temp); 75 | std::getline(ss, temp, ','); 76 | imu_data.linear_accel.z() = std::stod(temp); 77 | 78 | ss.clear(); 79 | ss << ref_accel_line; 80 | std::getline(ss, temp, ','); 81 | imu_data.true_linear_accel.x() = std::stod(temp); 82 | std::getline(ss, temp, ','); 83 | imu_data.true_linear_accel.y() = std::stod(temp); 84 | std::getline(ss, temp, ','); 85 | imu_data.true_linear_accel.z() = std::stod(temp); 86 | 87 | ss.clear(); 88 | ss << gyro_line; 89 | std::getline(ss, temp, ','); 90 | imu_data.angle_velocity.x() = std::stod(temp) * kDegree2Radian; 91 | std::getline(ss, temp, ','); 92 | imu_data.angle_velocity.y() = std::stod(temp) * kDegree2Radian; 93 | std::getline(ss, temp, ','); 94 | imu_data.angle_velocity.z() = std::stod(temp) * kDegree2Radian; 95 | 96 | ss.clear(); 97 | ss << ref_gyro_line; 98 | std::getline(ss, temp, ','); 99 | imu_data.true_angle_velocity.x() = std::stod(temp) * kDegree2Radian; 100 | std::getline(ss, temp, ','); 101 | imu_data.true_angle_velocity.y() = std::stod(temp) * kDegree2Radian; 102 | std::getline(ss, temp, ','); 103 | imu_data.true_angle_velocity.z() = std::stod(temp) * kDegree2Radian; 104 | 105 | ss.clear(); 106 | ss << ref_pos_line; 107 | std::getline(ss, temp, ','); 108 | imu_data.true_t_enu.x() = std::stod(temp); 109 | std::getline(ss, temp, ','); 110 | imu_data.true_t_enu.y() = std::stod(temp); 111 | std::getline(ss, temp, ','); 112 | imu_data.true_t_enu.z() = std::stod(temp); 113 | 114 | ss.clear(); 115 | ss << ref_att_quat_line; 116 | std::getline(ss, temp, ','); 117 | imu_data.true_q_enu.w() = std::stod(temp); 118 | std::getline(ss, temp, ','); 119 | imu_data.true_q_enu.x() = std::stod(temp); 120 | std::getline(ss, temp, ','); 121 | imu_data.true_q_enu.y() = std::stod(temp); 122 | std::getline(ss, temp, ','); 123 | imu_data.true_q_enu.z() = std::stod(temp); 124 | 125 | imu_data_buff.emplace_back(imu_data); 126 | } 127 | 128 | LOG(INFO) << "Read IMU data successfully"; 129 | } 130 | 131 | void IMUTool::ReadIMUData(const std::string &path, std::deque &imu_data_buff, const int skip_rows) { 132 | LOG(INFO) << "Read IMU data ..."; 133 | std::string accel_file_path = path + "/accel-0.csv"; 134 | std::string ref_accel_file_path = path + "/ref_accel.csv"; 135 | std::string gyro_file_path = path + "/gyro-0.csv"; 136 | std::string ref_gyro_file_path = path + "/ref_gyro.csv"; 137 | std::string time_file_path = path + "/time.csv"; 138 | std::string ref_pos_file_path = path + "/ref_pos.csv"; 139 | std::string ref_att_quat_file_path = path + "/ref_att_quat.csv"; 140 | 141 | std::ifstream accel_file(accel_file_path, std::ios::in); 142 | std::ifstream ref_accel_file(ref_accel_file_path, std::ios::in); 143 | std::ifstream gyro_file(gyro_file_path, std::ios::in); 144 | std::ifstream ref_gyro_file(ref_gyro_file_path, std::ios::in); 145 | std::ifstream ref_att_quat_file(ref_att_quat_file_path, std::ios::in); 146 | std::ifstream ref_pos_file(ref_pos_file_path, std::ios::in); 147 | std::ifstream time_file(time_file_path, std::ios::in); 148 | 149 | if (!accel_file.is_open() || !ref_accel_file.is_open() 150 | || !gyro_file.is_open() || !ref_gyro_file.is_open() 151 | || !time_file.is_open() || !ref_att_quat_file.is_open() 152 | || !ref_pos_file.is_open()) { 153 | LOG(FATAL) << "Failure to open IMU file"; 154 | } 155 | 156 | IMUData imu_data; 157 | imu_data_buff.clear(); 158 | 159 | std::string accel_line; 160 | std::string ref_accel_line; 161 | std::string gyro_line; 162 | std::string ref_gyro_line; 163 | std::string ref_pos_line; 164 | std::string ref_att_quat_line; 165 | std::string time_line; 166 | std::string temp; 167 | 168 | for (int i = 0; i < skip_rows; ++i) { 169 | std::getline(accel_file, temp); 170 | std::getline(ref_accel_file, temp); 171 | std::getline(gyro_file, temp); 172 | std::getline(ref_gyro_file, temp); 173 | std::getline(time_file, temp); 174 | std::getline(ref_pos_file, temp); 175 | std::getline(ref_att_quat_file, temp); 176 | } 177 | 178 | while (std::getline(accel_file, accel_line) && 179 | std::getline(ref_accel_file, ref_accel_line) && 180 | std::getline(gyro_file, gyro_line) && 181 | std::getline(ref_gyro_file, ref_gyro_line) && 182 | std::getline(time_file, time_line) && 183 | std::getline(ref_att_quat_file, ref_att_quat_line) && 184 | std::getline(ref_pos_file, ref_pos_line)) { 185 | imu_data.time = std::stod(time_line); 186 | 187 | std::stringstream ss; 188 | ss << accel_line; 189 | 190 | std::getline(ss, temp, ','); 191 | imu_data.linear_accel.x() = std::stod(temp); 192 | std::getline(ss, temp, ','); 193 | imu_data.linear_accel.y() = std::stod(temp); 194 | std::getline(ss, temp, ','); 195 | imu_data.linear_accel.z() = std::stod(temp); 196 | 197 | ss.clear(); 198 | ss << ref_accel_line; 199 | std::getline(ss, temp, ','); 200 | imu_data.true_linear_accel.x() = std::stod(temp); 201 | std::getline(ss, temp, ','); 202 | imu_data.true_linear_accel.y() = std::stod(temp); 203 | std::getline(ss, temp, ','); 204 | imu_data.true_linear_accel.z() = std::stod(temp); 205 | 206 | ss.clear(); 207 | ss << gyro_line; 208 | std::getline(ss, temp, ','); 209 | imu_data.angle_velocity.x() = std::stod(temp) * kDegree2Radian; 210 | std::getline(ss, temp, ','); 211 | imu_data.angle_velocity.y() = std::stod(temp) * kDegree2Radian; 212 | std::getline(ss, temp, ','); 213 | imu_data.angle_velocity.z() = std::stod(temp) * kDegree2Radian; 214 | 215 | ss.clear(); 216 | ss << ref_gyro_line; 217 | std::getline(ss, temp, ','); 218 | imu_data.true_angle_velocity.x() = std::stod(temp) * kDegree2Radian; 219 | std::getline(ss, temp, ','); 220 | imu_data.true_angle_velocity.y() = std::stod(temp) * kDegree2Radian; 221 | std::getline(ss, temp, ','); 222 | imu_data.true_angle_velocity.z() = std::stod(temp) * kDegree2Radian; 223 | 224 | ss.clear(); 225 | ss << ref_pos_line; 226 | std::getline(ss, temp, ','); 227 | imu_data.true_t_enu.x() = std::stod(temp); 228 | std::getline(ss, temp, ','); 229 | imu_data.true_t_enu.y() = std::stod(temp); 230 | std::getline(ss, temp, ','); 231 | imu_data.true_t_enu.z() = std::stod(temp); 232 | 233 | ss.clear(); 234 | ss << ref_att_quat_line; 235 | std::getline(ss, temp, ','); 236 | imu_data.true_q_enu.w() = std::stod(temp); 237 | std::getline(ss, temp, ','); 238 | imu_data.true_q_enu.x() = std::stod(temp); 239 | std::getline(ss, temp, ','); 240 | imu_data.true_q_enu.y() = std::stod(temp); 241 | std::getline(ss, temp, ','); 242 | imu_data.true_q_enu.z() = std::stod(temp); 243 | 244 | imu_data_buff.emplace_back(imu_data); 245 | } 246 | 247 | LOG(INFO) << "Read IMU data successfully"; 248 | } -------------------------------------------------------------------------------- /src/observability_analysis.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/3/1. 3 | // 4 | #include "observability_analysis.h" 5 | 6 | #include 7 | 8 | bool ObservabilityAnalysis::ComputeSOM(const int &interval, const int &number) { 9 | Qso_.resize(3*15*FGs_.size(), 15); 10 | std::cout << Qso_.rows() << " : " << Qso_.cols() << std::endl; 11 | Ys_.resize(3*15*FGs_.size(), 1); 12 | std::cout << Ys_.rows() << " : " << Ys_.cols() << std::endl; 13 | Eigen::Matrix Fnn = Eigen::Matrix::Identity(); 14 | for (int i = 0; i < FGs_.size(); ++i) 15 | { 16 | Eigen::Matrix Fn = Eigen::Matrix::Identity(); 17 | for (int j = 0; j < 15; j++) 18 | { 19 | if (j > 0) 20 | { 21 | Fn = Fn * FGs_[i].F; 22 | } 23 | Ys_.block<3, 1>(i*3*15+3*j, 0) = FGs_[i].Y[j]; 24 | Qso_.block<3, 15>(i*3*15+3*j, 0) = FGs_[i].G * Fn * Fnn; 25 | } 26 | } 27 | Eigen::FullPivLU lu_decomp(Qso_); 28 | auto rank = lu_decomp.rank(); 29 | std::cout << "matrix rank: " << rank << std::endl; 30 | 31 | return true; 32 | } 33 | 34 | bool ObservabilityAnalysis::ComputeObservability() { 35 | Eigen::JacobiSVD svd(Qso_, Eigen::ComputeFullU | Eigen::ComputeThinV); 36 | 37 | for (int i = 0; i < 15; ++i) { 38 | double temp = (svd.matrixU().row(i) * Ys_)[0] / svd.singularValues()[i]; 39 | Eigen::MatrixXd Xi = temp * svd.matrixV().col(i); 40 | Eigen::MatrixXd::Index max_row, max_col; 41 | Xi = Xi.cwiseAbs(); 42 | double max_value = Xi.maxCoeff(&max_row, &max_col); 43 | std::cout << svd.singularValues()[i] / svd.singularValues()[0] << "," << max_row << std::endl; 44 | } 45 | 46 | return true; 47 | } 48 | 49 | bool ObservabilityAnalysis::SaveFG(const TypeF &F, const TypeG &G, const TypeY &Y, const double& time) { 50 | static int FGSize = 10; 51 | static double time_interval = 100; 52 | 53 | if (FGs_.size() > FGSize) 54 | { 55 | return true; 56 | } 57 | if (FGs_.empty()) 58 | { 59 | FG fg; 60 | fg.time = time; 61 | // fg.F = Ft; 62 | fg.F = F - Eigen::Matrix::Identity(); 63 | // fg.F = (Ft - Eigen::Matrix::Identity()) / T; 64 | fg.G = G; 65 | fg.Y.push_back(Y); 66 | FGs_.push_back(fg); 67 | } 68 | else 69 | { 70 | if (FGs_.back().Y.size() == 15) 71 | { 72 | if (time - FGs_.back().time < time_interval || FGs_.size() >= FGSize) 73 | { 74 | return true; 75 | } 76 | FG fg; 77 | fg.time = time; 78 | // fg.F = Ft; 79 | fg.F = F - Eigen::Matrix::Identity(); 80 | // fg.F = (Ft - Eigen::Matrix::Identity()) / T; 81 | fg.G = G; 82 | fg.Y.push_back(Y); 83 | FGs_.push_back(fg); 84 | } 85 | else 86 | { 87 | FGs_.back().Y.push_back(Y); 88 | } 89 | 90 | } 91 | return true; 92 | } -------------------------------------------------------------------------------- /test/test_gps.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/2/23. 3 | // 4 | 5 | #include 6 | 7 | #include "gps_tool.h" 8 | 9 | int main(int argc, char **argv) { 10 | if (argc != 2) { 11 | std::cout << "Please enter right command!" << std::endl; 12 | std::cout << "$ ./test_gps data_file_path" << std::endl; 13 | 14 | return -1; 15 | } 16 | 17 | std::string data_path = std::string(argv[1]); 18 | 19 | std::vector gps_data_buff; 20 | 21 | GPSTool gps_tool(0.0, 0.0, 0.0); 22 | gps_tool.ReadGPSData(data_path, gps_data_buff); 23 | 24 | 25 | for (int i = 0; i < gps_data_buff.size(); ++i) { 26 | std::cout << "\nindex: " << i << std::endl; 27 | std::cout << "time:" << std::to_string(gps_data_buff.at(i).time) << std::endl; 28 | std::cout << "posi(LLA): " << gps_data_buff.at(i).position_lla.transpose() << std::endl; 29 | std::cout << "posi(NED): " << gps_data_buff.at(i).local_position_ned.transpose() << std::endl; 30 | std::cout << "velocity: " << gps_data_buff.at(i).velocity.transpose() << std::endl; 31 | std::cout << "true velocity" << gps_data_buff.at(i).true_velocity.transpose() << std::endl; 32 | } 33 | 34 | return 0; 35 | } 36 | 37 | -------------------------------------------------------------------------------- /test/test_imu.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by meng on 2021/2/23. 3 | // 4 | 5 | #include 6 | 7 | #include "imu_tool.h" 8 | 9 | int main(int argc, char **argv) { 10 | if (argc != 2) { 11 | std::cout << "Please enter right command!" << std::endl; 12 | std::cout << "$ ./test_imu data_file_path" << std::endl; 13 | 14 | return -1; 15 | } 16 | 17 | std::vector imu_data_buff; 18 | 19 | IMUTool::ReadIMUData(std::string(argv[1]), imu_data_buff); 20 | 21 | for (int i = 0; i < imu_data_buff.size(); ++i) { 22 | std::cout << "\nindex: " << i << std::endl; 23 | std::cout << "time: " << std::to_string(imu_data_buff.at(i).time) << std::endl; 24 | std::cout << "ref_gyro: " << imu_data_buff.at(i).true_angle_velocity.transpose() << std::endl; 25 | std::cout << "gyro: " << imu_data_buff.at(i).angle_velocity.transpose() << std::endl; 26 | std::cout << "accel: " << imu_data_buff.at(i).linear_accel.transpose() << std::endl; 27 | } 28 | 29 | return 0; 30 | } 31 | 32 | -------------------------------------------------------------------------------- /test/test_tool.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Zhang Zhimeng on 23-7-16. 3 | // 4 | #include 5 | #include "common_tool.h" 6 | 7 | int main(int argc, char **argv) { 8 | Eigen::Vector3d V_0_Ned(1, 2, 3); 9 | Eigen::Vector3d V_0_Enu = NedToEnu(V_0_Ned); 10 | 11 | std::cout << "Test 0" << std::endl; 12 | std::cout << "V_0_Ned: " << V_0_Ned.transpose() << std::endl; 13 | std::cout << "V_0_Enu: " << V_0_Enu.transpose() << std::endl << std::endl; 14 | 15 | Eigen::Vector3d V_1_Ned(0, 0, 0); 16 | Eigen::Vector3d V_1_Enu = NedToEnu(V_1_Ned); 17 | 18 | std::cout << "Test 1" << std::endl; 19 | std::cout << "V_1_Ned: " << V_1_Ned.transpose() << std::endl; 20 | std::cout << "V_1_Enu: " << V_1_Enu.transpose() << std::endl << std::endl; 21 | 22 | Eigen::Vector3d V_2_Ned(0, 0.1, 100); 23 | Eigen::Vector3d V_2_Enu = NedToEnu(V_2_Ned); 24 | 25 | std::cout << "Test 2" << std::endl; 26 | std::cout << "V_2_Ned: " << V_2_Ned.transpose() << std::endl; 27 | std::cout << "V_2_Enu: " << V_2_Enu.transpose() << std::endl; 28 | 29 | return 0; 30 | } 31 | --------------------------------------------------------------------------------