├── .gitignore ├── .gitlab-ci.yml ├── CHANGELOG.md ├── CMakeLists.txt ├── CONTRIBUTING.md ├── LICENSE.md ├── README.md ├── docs └── 00481290.pdf ├── examples ├── arduino │ └── nav_example │ │ └── nav_example.ino └── cmake │ └── nav_example.cc ├── img ├── arduino_logo_75.png └── logo-words_75.png ├── keywords.txt ├── library.properties └── src ├── constants.h ├── earth_model.h ├── ekf_15_state.h ├── navigation.h ├── tilt_compass.h ├── transforms.h └── utils.h /.gitignore: -------------------------------------------------------------------------------- 1 | # ignore the build directory 2 | /build 3 | # ignore vscode 4 | *.vscode 5 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | stages: 2 | - lint 3 | 4 | Lint: 5 | stage: lint 6 | tags: 7 | - bfs 8 | script: 9 | - cpplint --verbose=0 src/constants.h 10 | - cpplint --verbose=0 src/ekf_15_state.h 11 | - cpplint --verbose=0 src/navigation.h 12 | - cpplint --verbose=0 src/tilt_compass.h 13 | - cpplint --verbose=0 src/transforms.h 14 | - cpplint --verbose=0 src/utils.h 15 | - cpplint --verbose=0 src/ekf_15_state.cpp 16 | - cpplint --verbose=0 src/tilt_compass.cpp 17 | - cpplint --verbose=0 src/transforms.cpp 18 | - cpplint --verbose=0 src/utils.cpp 19 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # Changelog 2 | 3 | ## v4.0.2 4 | - Typo / bug in EKF15 state didn't take the sqrt of the denominator in the radius of curvature calc 5 | 6 | ## v4.0.1 7 | - Fixed inline-ness of functions 8 | 9 | ## v4.0.0 10 | - Updated function signatures to match MATLAB 11 | - Added README 12 | 13 | ## v3.0.0 14 | - Updated to work with CMake and Arduino build systems 15 | 16 | ## v2.0.1 17 | - Updated to eigen v2.0.0 and units v3.2.0 18 | 19 | ## v2.0.0 20 | - Updated namespace to *bfs* 21 | 22 | ## v1.2.4 23 | - Updated CONTRIBUTING 24 | - Switched dependencies to GitHub source 25 | - Moved navigation::constants to navigation 26 | 27 | ## v1.2.3 28 | - Updated CONTRIBUTING 29 | - Switched from ssh to https for *fetch_content* 30 | 31 | ## v1.2.2 32 | - Updated to MIT license 33 | - Specified versions of dependencies 34 | 35 | ## v1.2.1 36 | - Fixed bugs with ConstrainPi and Constrain2Pi. 37 | 38 | ## v1.2.0 39 | - Added ConstrainPi and Constrain2Pi, which can convert angles to +/-Pi or 0-2Pi respectively. 40 | 41 | ## v1.1.0 42 | 43 | - Added Eigen::Vector3d output for lla 44 | 45 | ## v1.0.0 46 | 47 | - Initial baseline 48 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | # Project information 3 | project(Navigation 4 | VERSION 4.0.2 5 | DESCRIPTION "Navigation filters and utilities" 6 | LANGUAGES CXX 7 | ) 8 | include(FetchContent) 9 | # Fetch eigen 10 | FetchContent_Declare( 11 | eigen 12 | GIT_REPOSITORY https://github.com/bolderflight/eigen.git 13 | GIT_TAG v3.0.2 14 | ) 15 | FetchContent_MakeAvailable(eigen) 16 | #Fetch units 17 | FetchContent_Declare( 18 | units 19 | GIT_REPOSITORY https://github.com/bolderflight/units.git 20 | GIT_TAG v4.1.0 21 | ) 22 | FetchContent_MakeAvailable(units) 23 | # Add the library target 24 | add_library(navigation INTERFACE) 25 | target_link_libraries(navigation 26 | INTERFACE 27 | eigen 28 | units 29 | ) 30 | # Setup include directories 31 | target_include_directories(navigation INTERFACE src) 32 | 33 | # Example and unit testing if this project is built separately 34 | if (PROJECT_NAME STREQUAL CMAKE_PROJECT_NAME) 35 | # Add the example target 36 | add_executable(nav_example examples/cmake/nav_example.cc) 37 | # Add the includes 38 | target_include_directories(nav_example PUBLIC 39 | $ 40 | $ 41 | ) 42 | # Link libraries to the test target 43 | target_link_libraries(nav_example 44 | PRIVATE 45 | navigation 46 | ) 47 | endif() 48 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contributing 2 | 3 | Please see our [Contributing Guide](https://github.com/bolderflight/contributing) for tips on how to effectively make contributions to our project. 4 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2022 Bolder Flight Systems Inc 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | 7 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | 9 | THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 10 | 11 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![License](https://img.shields.io/badge/License-BSD_3--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) 2 | 3 | ![Bolder Flight Systems Logo](img/logo-words_75.png)     ![Arduino Logo](img/arduino_logo_75.png) 4 | 5 | # Navigation 6 | 7 | * [License](LICENSE.md) 8 | * [Changelog](CHANGELOG.md) 9 | * [Contributing guide](CONTRIBUTING.md) 10 | 11 | # Description 12 | 13 | # Installation 14 | 15 | ## Arduino 16 | Simply clone or download and extract the zipped library into your Arduino/libraries folder. In addition to this library, the [Bolder Flight Systems Units library](https://github.com/bolderflight/units) and [Bolder Flight Systems Eigen library](https://github.com/bolderflight/eigen) must be installed. The library is added as: 17 | 18 | ```C++ 19 | #include "navigation.h" 20 | ``` 21 | 22 | An example is located in *examples/arduino/nav_example/nav_example.ino*. This library is tested with Teensy 3.x, 4.x, and LC devices and is expected to work with other Arduino ARM devices. It is **not** expected to work with AVR devices. 23 | 24 | ## CMake 25 | CMake is used to build this library, which is exported as a library target called *navigation*. The header is added as: 26 | 27 | ```C++ 28 | #include "navigation.h" 29 | ``` 30 | 31 | The library can be also be compiled stand-alone using the CMake idiom of creating a build directory and then, from within that directory issuing: 32 | 33 | ``` 34 | cmake .. 35 | make 36 | ``` 37 | 38 | This will build the library, an example executable called *nav_example*, and an executable for testing using the Google Test framework, called *nav_test*. The example executable source file is located at *examples/cmake/nav_example.cc*. 39 | 40 | # Namespace 41 | This library is within the namespace *bfs*. 42 | 43 | # Constants 44 | The following constants are defined from WGS84: 45 | 46 | | Description | Variable | Value | 47 | | --- | --- | --- | 48 | | Semi-major axis, m | double SEMI_MAJOR_AXIS_LENGTH_M | 6378137.0 | 49 | | Semi-minor axis, m | double SEMI_MINOR_AXIS_LENGTH_M | 6356752.3142 | 50 | | Flattening | double FLATTENING | 1.0 / 298.257223563 | 51 | | First eccentricity | double ECC | 8.1819190842622e-2 | 52 | | First eccentricity squared | double ECC2 | 6.69437999014e-3 | 53 | | Angular velocity of the Earth, rad/s | WE_RADPS | 7292115.0e-11 | 54 | | Earth's gravitational constant, m^3/s^2 | GM_M3PS2 | 3986004.418e8 | 55 | 56 | # Filters 57 | 58 | ## Tilt Compass 59 | 60 | **Eigen::Vector3f TiltCompass(const Eigen::Vector3f &accel, const Eigen::Vector3f &mag)** Estimates heading, pitch, and roll given 3-axis accelerometer and magnetometer data. Accelerometer and magnetometer data is expected to be oriented with the positive x-axis forward, y-axis out the right, and z-axis down. Heading, pitch, and roll are output in radians. 61 | 62 | ## 15 State EKF INS 63 | 64 | # Transformations 65 | 66 | ## Position 67 | These functions convert between: 68 | * Latitude, longitude, and altitude above the WGS84 ellipsoid (LLA) 69 | * Earth Centered Earth Fixed (ECEF) 70 | * Locally tangent North East Down (NED) 71 | 72 | **Eigen::Vector3d lla2ecef(const Eigen::Vector3d &lla, const AngPosUnit ang = AngPosUnit::DEG)** Convert geodetic coordinates to Earth-centered Earth-fixed (ECEF) coordinates. Inputs are latitude, longitude, and altitude [m] above the WGS84 ellipsoid. Optionally, the units of latitude and longitude can be specified; by default, they are in degrees. Outputs are ECEF coordinates [m]. 73 | 74 | ```C++ 75 | Eigen::Vector3d lla = {0, 45, 1000}; 76 | Eigen::Vector3d ecef = bfs::lla2ecef(lla); 77 | ``` 78 | 79 | **Eigen::Vector3d ecef2lla(const Eigen::Vector3d &ecef, const AngPosUnit ang = AngPosUnit::DEG)** Convert Earth-centered Earth-fixed (ECEF) coordinates to geodetic coordinates. Inputs are ECEF coordinates [m] and, optionally, the units for the output latitude and longitude; by default they are in degrees. Uses [Olson's method](https://ieeexplore.ieee.org/abstract/document/481290), which is iteration-free, very fast, and accurate. 80 | 81 | ```C++ 82 | Eigen::Vector3d ecef = {4510731, 4510731, 0}; 83 | Eigen::Vector3d lla = bfs::ecef2lla(ecef); 84 | ``` 85 | 86 | **Eigen::Vector3d ecef2ned(const Eigen::Vector3d &ecef, const Eigen::Vector3d &lla0, const AngPosUnit ang = AngPosUnit::DEG)** Rotates an Earth Centered Earth Fixed (ECEF) vector into a locally tangent North East Down (NED) frame. Inputs are the ECEF vector and the origin of the NED frame, given as latitude, longitude, and altitude [m] above the WGS84 ellipsoid. Optionally, the units of latitude and longitude can be specified; by default, they are in degrees. 87 | 88 | ```C++ 89 | Eigen::Vector3d ecef = {4510731, 4510731, 0}; 90 | Eigen::Vector3d lla0 = {0, 45, 999.956}; 91 | Eigen::Vector3d ned = bfs::ecef2ned(ecef, lla0); 92 | ``` 93 | 94 | **Eigen::Vector3d ned2ecef(const Eigen::Vector3d &ned, const Eigen::Vector3d &lla_ref, const AngPosUnit ang = AngPosUnit::DEG)** Rotates a North East Down (NED) vector into Earth Centered Earth Fixed (ECEF). Inputs are the NED vector and the origin of the NED frame, given as latitude, longitude, and altitude [m] above the WGS84 ellipsoid. Optionally, the units of latitude and longitude can be specified; by default, they are in degrees. 95 | 96 | ```C++ 97 | Eigen::Vector3d ned = {20, 20, 0}; 98 | Eigen::Vector3d lla0 = {0, 45, 999.956}; 99 | Eigen::Vector3d ecef = bfs::ned2ecef(ned, lla0); 100 | ``` 101 | 102 | **Eigen::Vector3d lla2ned(const Eigen::Vector3d &lla, const Eigen::Vector3d &lla0, const AngPosUnit ang = AngPosUnit::DEG)** Transforms geodetic coordinates to locally tangent North East Down (NED) coordinates. Inputs are the latitude, longitude, and altitude [m] above the WGS84 ellipsoid of the coordinates and latitude, longitude, and altitude [m] above the WGS84 ellipsoid of the origin of the NED frame. Optionally, the units of latitude and longitude can be specified; by default, they are in degrees. 103 | 104 | ```C++ 105 | Eigen::Vector3d lla0 = {46.017, 7.750, 1673}; 106 | Eigen::Vector3d lla = {45.976, 7.658, 4531}; 107 | Eigen::Vector3d ned = bfs::lla2ned(lla, lla0); 108 | ``` 109 | 110 | **Eigen::Vector3d ned2lla(const Eigen::Vector3d &ned, const Eigen::Vector3d &lla0, const AngPosUnit ang = AngPosUnit::DEG)** Transforms local North East Down (NED) coordinates to geodetic coordinates. Inputs are the local NED coordinates and the latitude, longitude, and altitude [m] above the WGS84 ellipsoid of the origin of the NED frame. Optionally, the units of latitude and longitude can be specified; by default, they are in degrees. 111 | 112 | ```C++ 113 | Eigen::Vector3d lla0 = {46.017, 7.750, 1673}; 114 | Eigen::Vector3d ned = {-4556.3, -7134.8, -2852.4}; 115 | Eigen::Vector3d lla = bfs::ned2lla(ned, lla0); 116 | ``` 117 | 118 | ## Attitude 119 | These functions convert between: 120 | * Rotation angle series 121 | * Euler angles ('ZYX' order rotation angles, i.e. yaw, pitch, roll) 122 | * Direction cosine matrix 123 | * Quaternions (i, j, k, w) 124 | 125 | **Eigen::Matrix angle2dcm(const T rot1, const T rot2, const T rot3, const AngPosUnit ang = AngPosUnit::RAD)** Converts a series of rotation angles to a direction cosine matrix. Inputs are the three rotation angles, and optionally, the units of those angles. By default, angle input is expected in radians. Templated by floating point type. 126 | 127 | ```C++ 128 | float yaw = 0.7854; 129 | float pitch = 0.1; 130 | float roll = 0; 131 | Eigen::Matrix3f dcm = bfs::angle2dcm(yaw, pitch, roll); 132 | ``` 133 | 134 | **Eigen::Matrix eul2dcm(const Eigen::Matrix &eul, const AngPosUnit ang = AngPosUnit::RAD)** Converts a given set of Euler angles to a direction cosine matrix. Input is the Euler angle vector (order 'ZYX') and optionally, the units of those angles. By default, angle input is expected in radians. Templated by floating point type. 135 | 136 | ```C++ 137 | float yaw = 0.7854; 138 | float pitch = 0.1; 139 | float roll = 0; 140 | Eigen::Vector3f eul = {yaw, pitch, roll}; 141 | Eigen::Matrix3f dcm = bfs::eul2dcm(eul); 142 | ``` 143 | 144 | **Eigen::Matrix dcm2angle(const Eigen::Matrix &dcm, const AngPosUnit ang = AngPosUnit::RAD)** Creates a series of rotation angles from direction cosine matrix. Inputs are the direction cosine matrix, and optionally, the units of the output rotation angles. By default the angles are output in radians. Templated by floating point type. 145 | 146 | ```C++ 147 | Eigen::Matrix3f dcm; 148 | dcm << 0.85253103550038, 0.47703040785184, -0.21361840626067, 149 | -0.43212157513194, 0.87319830445628, 0.22537893734811, 150 | 0.29404383655186, -0.09983341664683, 0.95056378592206; 151 | Eigen::Vector3f ang = bfs::dcm2angle(dcm); 152 | ``` 153 | 154 | **Eigen::Matrix dcm2eul(const Eigen::Matrix &dcm, const AngPosUnit ang = AngPosUnit::RAD)** Creates a set of Euler angles (order 'ZYX') from direction cosine matrix. Inputs are the direction cosine matrix, and optionally, the units of the output Euler angles. By default the angles are output in radians. Templated by floating point type. Functionally equivalent to *dcm2angle*. 155 | 156 | ```C++ 157 | Eigen::Matrix3f dcm; 158 | dcm << 0.85253103550038, 0.47703040785184, -0.21361840626067, 159 | -0.43212157513194, 0.87319830445628, 0.22537893734811, 160 | 0.29404383655186, -0.09983341664683, 0.95056378592206; 161 | Eigen::Vector3f ang = bfs::dcm2eul(dcm); 162 | ``` 163 | 164 | **Eigen::Quaternion angle2quat(const T rot1, const T rot2, const T rot3, const AngPosUnit a = AngPosUnit::RAD)** Convert rotation angles to quaternion. Inputs are the three rotation angles, and optionally, the units of those angles. By default, angle input is expected in radians. Templated by floating point type. 165 | 166 | ```C++ 167 | float yaw = 0.7854; 168 | float pitch = 0.1; 169 | float roll = 0; 170 | Eigen::Quaternionf q = bfs::angle2quat(yaw, pitch, roll); 171 | ``` 172 | 173 | **Eigen::Quaternion eul2quat(const T rot1, const T rot2, const T rot3, const AngPosUnit a = AngPosUnit::RAD)** Converts a given set of Euler angles to quaternion. Input is the Euler angle vector (order 'ZYX'), and optionally, the units of those angles. By default, angle input is expected in radians. Templated by floating point type. 174 | 175 | ```C++ 176 | float yaw = 0.7854; 177 | float pitch = 0.1; 178 | float roll = 0; 179 | Eigen::Vector3f eul = {yaw, pitch, roll}; 180 | Eigen::Quaternionf q = bfs::eul2quat(eul); 181 | ``` 182 | 183 | **Eigen::Matrix quat2angle(const Eigen::Quaternion &q, const AngPosUnit ang = AngPosUnit::RAD)** Converts a quaternion to a series of rotation angles. Input is the quaternion, and optionally, the units of the output rotation angles. By default the angles are output in radians. Templated by floating point type. 184 | 185 | ```C++ 186 | Eigen::Quaternionf q; 187 | q.x() = 0.215509; 188 | q.y() = 0.432574; 189 | q.z() = 0.0846792; 190 | q.w() = 0.871358; 191 | Eigen::Vector3f ang = bfs::quat2angle(q); 192 | ``` 193 | 194 | **Eigen::Matrix quat2eul(const Eigen::Quaternion &q, const AngPosUnit ang = AngPosUnit::RAD)** Converts a quaternion to Euler angles. Input is the quaternion, and optionally, the units of the output Euler angles. By default the angles are output in radians. Templated by floating point type. Functionally equivalent to *quat2angle*. 195 | 196 | ```C++ 197 | Eigen::Quaternionf q; 198 | q.x() = 0.215509; 199 | q.y() = 0.432574; 200 | q.z() = 0.0846792; 201 | q.w() = 0.871358; 202 | Eigen::Vector3f ang = bfs::quat2eul(q); 203 | ``` 204 | 205 | **Eigen::Quaternion dcm2quat(const Eigen::Matrix &dcm)** Convert direction cosine matrix to quaternion. Input is the direction cosine matrix and output is the quaternion. Templated by floating point type. 206 | 207 | ```C++ 208 | Eigen::Matrix3f dcm; 209 | dcm << 0.4330, 0.2500, -0.8660, 210 | 0.1768, 0.9186, 0.3536, 211 | 0.8839, -0.3062, 0.3536; 212 | Eigen::Quaternionf q = bfs::dcm2quat(dcm); 213 | ``` 214 | 215 | **Eigen::Matrix quat2dcm(const Eigen::Quaternion &q)** Convert quaternion to direction cosine matrix. Input is the quaternion and output is the direction cosine matrix. Templated by floating point type. 216 | 217 | ```C++ 218 | Eigen::Quaternionf q; 219 | q.x() = 0.200578; 220 | q.y() = 0.531966; 221 | q.z() = 0.0222526; 222 | q.w() = 0.822375; 223 | Eigen::Matrix3f dcm = bfs::quat2dcm(q); 224 | ``` 225 | 226 | # Earth Modeling 227 | The following functions are converted from and tested against [NavPy](https://github.com/NavPy/NavPy). They compute the Earth's radius of curvature, LLA rate, Earth rotation rate resolved on a local NED frame, and the navigation rate. 228 | 229 | **double earthrad_transverse_m(const double lat, const AngPosUnit ang = AngPosUnit::DEG)** Computes the radius of curvature [m] in the prime-vertical (i.e. transverse or east-west) direction given a latitude and, optionally, the latitude unit. If the latitude unit is not specified, it is in degrees by default. 230 | 231 | **double earthrad_meridonal_m(const double lat, const AngPosUnit ang = AngPosUnit::DEG)** Computes the radius of curvature [m] in the meridonal (i.e. north-south) direction given a latitude and, optionally, the latitude unit. If the latitude unit is not specified, it is in degrees by default. 232 | 233 | **std::array earthrad_m(const double lat, const AngPosUnit ang = AngPosUnit::DEG)** Computes the radius of curvature in the transverse and meridonal directions, returning the result as an array. 234 | 235 | **Eigen::Vector3d llarate(const double vn, const double ve, const double vd, const double lat, const double alt, const AngPosUnit ang = AngPosUnit::DEG)** Computes the latitude, longitude, and altitude (LLA) rate given the locally tangent North-East-Down (NED) velocity [m/s] components, the latitude, altitude above the WGS84 ellipsoid [m], and, optionally, the latitude unit. If the latitude unit is not specified, it is in degrees by default. The latitude and longitude rate are given in deg/s if the latitude is given in degrees and rad/s if the latitude is given in radians. 236 | 237 | **Eigen::Vector3f llarate(const Eigen::Vector3f &ned_vel, const Eigen::Vector3d &lla, const AngPosUnit ang = AngPosUnit::DEG)** Same functionality as the previous llarate function, but uses vectors for NED velocity and LLA input. 238 | 239 | **Eigen::Vector3d earthrate(const double lat, const AngPosUnit ang = AngPosUnit::DEG)** Computes the Earth rotation rate resolved on the locally tangent North-East-Down (NED) frame, given a latitude and, optionally, the latitude unit. If the latitude unit is not specified, it is in degrees by default. 240 | 241 | **Eigen::Vector3d navrate(const double vn, const double ve, const double vd, const double lat, const double alt, const AngPosUnit ang = AngPosUnit::DEG)** Computes the navigation/transport rate given the locally tangent North-East-Down (NED) velocity [m/s] components, the latitude, altitude above the WGS84 ellipsoid [m], and, optionally, the latitude unit. If the latitude unit is not specified, it is in degrees by default. The navigation/transport rate is the angular velocity of the NED frame relative to the earth ECEF frame. The navigation rate is given in deg/s if the latitude is given in degrees and rad/s if the latitude is given in radians. 242 | 243 | **Eigen::Vector3f navrate(const Eigen::Vector3f &ned_vel, const Eigen::Vector3d &lla, const AngPosUnit ang = AngPosUnit::DEG)** Same functionality as the previous navrate function, but uses vectors for NED velocity and LLA input. 244 | 245 | # Utilities 246 | 247 | **T WrapTo2Pi(T ang)** Converts a +/-PI radian angle to a 0 to 2PI angle. Templated by floating point type. 248 | 249 | **T WrapToPi(T ang)** Converts a 0 to 2PI radian angle to a +/-PI angle. Templated by floating point type. 250 | 251 | **Eigen::Matrix Skew(const Eigen::Matrix &w)** Creates a skew symmetric matrix from a vector. Templated by floating point type. 252 | -------------------------------------------------------------------------------- /docs/00481290.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bolderflight/navigation/9029646dc2ceb4455c467475743b61dad765240f/docs/00481290.pdf -------------------------------------------------------------------------------- /examples/arduino/nav_example/nav_example.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2021 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #include "navigation.h" 27 | 28 | void setup() { 29 | Eigen::Vector3d p0, p1, ref, l0, l1; 30 | 31 | p0 = {-27.4215, -29.3987, -6.4839}; 32 | p1 = {151.360, 90.000, 18.309}; 33 | ref = {bfs::deg2rad(35.15049), bfs::deg2rad(-106.732339), 2000.0}; 34 | l0 = bfs::ned2lla(p0, ref); 35 | l1 = bfs::ned2lla(p1, ref); 36 | Serial.println(l0(0)); 37 | Serial.println(l0(1)); 38 | Serial.println(l0(2)); 39 | 40 | // std::cout << std::setprecision(14) << l1(0) * 180.0 / M_PI << std::endl; 41 | // std::cout << std::setprecision(14) << l1(1) * 180.0 / M_PI << std::endl; 42 | // std::cout << std::setprecision(14) << l1(2) << std::endl; 43 | 44 | // navigation::Ekf15State ekf; 45 | // types::Imu imu; 46 | // ekf.TimeUpdate(imu, 20); 47 | // Eigen::Vector3d lla; 48 | // lla(0) = 35.679862 * M_PI / 180.0; 49 | // lla(1) = -105.962417 * M_PI / 180.0; 50 | // lla(2) = 4000.0; 51 | 52 | // Eigen::Vector3d ecef = navigation::lla2ecef(lla); 53 | 54 | // Eigen::Vector3d lla2 = navigation::ecef2lla(ecef); 55 | 56 | // std::cout << std::setprecision(14) << lla2(0) * 180.0 / M_PI << std::endl; 57 | // std::cout << std::setprecision(14) << lla2(1) * 180.0 / M_PI << std::endl; 58 | // std::cout << std::setprecision(14) << lla2(2) << std::endl; 59 | 60 | // std::cout << nav::constants::ECC2 << std::endl; 61 | 62 | // Eigen::Vector3d ypr; 63 | // ypr(0) = 80 * M_PI / 180; 64 | // ypr(1) = 20 * M_PI / 180; 65 | // ypr(2) = 10 * M_PI / 180; 66 | 67 | // Eigen::Quaterniond q = nav::Euler_to_Quat(ypr); 68 | 69 | // Eigen::Vector3d eul = nav::Quat_to_Euler(q); 70 | 71 | // std::cout << ypr << std::endl << std::endl; 72 | // std::cout << eul << std::endl << std::endl; 73 | 74 | // Eigen::Matrix3d dcm = nav::Euler_to_Dcm(ypr); 75 | // Eigen::Matrix3d dcm2 = nav::Quat_to_Dcm(q); 76 | 77 | // Eigen::Vector3d eul2 = nav::Dcm_to_Euler(dcm); 78 | // Eigen::Vector3d eul3 = nav::Dcm_to_Euler(dcm2); 79 | 80 | // std::cout << eul2 << std::endl << std::endl; 81 | // std::cout << eul3 << std::endl << std::endl; 82 | 83 | // Eigen::Quaterniond q2 = nav::Dcm_to_Quat(dcm); 84 | // Eigen::Quaterniond q3 = nav::Dcm_to_Quat(dcm2); 85 | 86 | // // Eigen::Quaterniond q2 = nav::Dcm_to_Quat(dcm); 87 | 88 | // std::cout << q.w() << std::endl; 89 | // std::cout << q.x() << std::endl; 90 | // std::cout << q.y() << std::endl; 91 | // std::cout << q.z() << std::endl << std::endl; 92 | // std::cout << q2.w() << std::endl; 93 | // std::cout << q2.x() << std::endl; 94 | // std::cout << q2.y() << std::endl; 95 | // std::cout << q2.z() << std::endl << std::endl; 96 | // std::cout << q3.w() << std::endl; 97 | // std::cout << q3.x() << std::endl; 98 | // std::cout << q3.y() << std::endl; 99 | // std::cout << q3.z() << std::endl << std::endl; 100 | 101 | } 102 | 103 | void loop() {} 104 | -------------------------------------------------------------------------------- /examples/cmake/nav_example.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2021 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #include "navigation.h" 27 | #include "units.h" 28 | #include 29 | // #include 30 | 31 | int main() { 32 | // std::cout << bfs::BFS_PI << std::endl; 33 | // std::cout << bfs::A1 << std::endl; 34 | // Eigen::Matrix3f dcm; 35 | // dcm << 0.4330, 0.2500, -0.8660, 36 | // 0.1768, 0.9186, 0.3536, 37 | // 0.8839, -0.3062, 0.3536; 38 | // Eigen::Quaternionf q = bfs::dcm2quat(dcm); 39 | // Eigen::Matrix3f dcm2 = bfs::quat2dcm(q); 40 | // std::cout << q << std::endl; 41 | // std::cout << dcm2 << std::endl; 42 | // // std::cout << ang << std::endl; 43 | // Eigen::Vector3d lla = {0, 45, 1000}; 44 | // Eigen::Vector3d ecef = bfs::lla2ecef(lla); 45 | // std::cout << ecef << std::endl; 46 | 47 | // Eigen::Vector3d ecef = {4510731, 4510731, 0}; 48 | // Eigen::Vector3d lla = bfs::ecef2lla(ecef); 49 | // std::cout << lla << std::endl; 50 | 51 | // Eigen::Vector3d ecef = {4510731, 4510731, 4510731}; 52 | // Eigen::Vector3d lla0 = {35, -105, 1000}; 53 | // Eigen::Vector3d lla1 = {35, -105, 1000}; 54 | // Eigen::Vector3d ecef = bfs::lla2ecef(lla1); 55 | // Eigen::Vector3d ned = bfs::ecef2ned(ecef, lla0); 56 | // std::cout << ned << std::endl; 57 | 58 | // Eigen::Vector3d lla0 = {46.017, 7.750, 1673}; 59 | // Eigen::Vector3d lla = {45.976, 7.658, 4531}; 60 | // Eigen::Vector3d ned = bfs::lla2ned(lla, lla0); 61 | // std::cout << ned << std::endl; 62 | 63 | Eigen::Vector3d lla0 = {46.017, 7.750, 1673}; 64 | Eigen::Vector3d ned = {-4556.3, -7134.8, -2852.4}; 65 | Eigen::Vector3d lla = bfs::ned2lla(ned, lla0); 66 | 67 | std::cout << lla << std::endl; 68 | 69 | // Eigen::Vector3d lla = {44.532, -72.782, 1699}; 70 | // Eigen::Vector3d ecef = {1345.660, -4350.891, 4452.314}; 71 | // ecef = ecef * 1000; 72 | // Eigen::Vector3d ned = bfs::ecef2ned(ecef, lla); 73 | // std::cout << ned << std::endl; 74 | // float pitch = 0.8; 75 | // float roll = 0.7; 76 | // float yaw = 0.5; 77 | // // Eigen::Quaternionf q = bfs::angle2quat(yaw, pitch, roll); 78 | // // std::cout << q << std::endl; 79 | // // Eigen::Vector3f ang = bfs::quat2angle(q); 80 | // // std::cout << ang << std::endl; 81 | 82 | // Eigen::Quaternionf q; 83 | // q.x() = 0.215509; 84 | // q.y() = 0.432574; 85 | // q.z() = 0.0846792; 86 | // q.w() = 0.871358; 87 | // Eigen::Vector3f ang = bfs::quat2eul(q); 88 | // std::cout << ang << std::endl; 89 | 90 | // Eigen::Quaternionf q = {0.3, 0.1, 1, 0.5}; 91 | // Eigen::Vector3f ang = bfs::quat2angle(q); 92 | // std::cout << ang << std::endl; 93 | 94 | // Eigen::Vector3d p0, p1, ref, l0, l1; 95 | 96 | // p0 = {-27.4215, -29.3987, -6.4839}; 97 | // p1 = {151.360, 90.000, 18.309}; 98 | // ref = {bfs::deg2rad(35.15049), bfs::deg2rad(-106.732339), 2000.0}; 99 | // l0 = bfs::ned2lla(p0, ref); 100 | // l1 = bfs::ned2lla(p1, ref); 101 | // std::cout << std::setprecision(14) << l0(0) * 180.0 / M_PI << std::endl; 102 | // std::cout << std::setprecision(14) << l0(1) * 180.0 / M_PI << std::endl; 103 | // std::cout << std::setprecision(14) << l0(2) << std::endl; 104 | 105 | // std::cout << std::setprecision(14) << l1(0) * 180.0 / M_PI << std::endl; 106 | // std::cout << std::setprecision(14) << l1(1) * 180.0 / M_PI << std::endl; 107 | // std::cout << std::setprecision(14) << l1(2) << std::endl; 108 | 109 | // navigation::Ekf15State ekf; 110 | // types::Imu imu; 111 | // ekf.TimeUpdate(imu, 20); 112 | // Eigen::Vector3d lla; 113 | // lla(0) = 35.679862 * M_PI / 180.0; 114 | // lla(1) = -105.962417 * M_PI / 180.0; 115 | // lla(2) = 4000.0; 116 | 117 | // Eigen::Vector3d ecef = navigation::lla2ecef(lla); 118 | 119 | // Eigen::Vector3d lla2 = navigation::ecef2lla(ecef); 120 | 121 | // std::cout << std::setprecision(14) << lla2(0) * 180.0 / M_PI << std::endl; 122 | // std::cout << std::setprecision(14) << lla2(1) * 180.0 / M_PI << std::endl; 123 | // std::cout << std::setprecision(14) << lla2(2) << std::endl; 124 | 125 | // std::cout << nav::constants::ECC2 << std::endl; 126 | 127 | // Eigen::Vector3d ypr; 128 | // ypr(0) = 80 * M_PI / 180; 129 | // ypr(1) = 20 * M_PI / 180; 130 | // ypr(2) = 10 * M_PI / 180; 131 | 132 | // Eigen::Quaterniond q = nav::Euler_to_Quat(ypr); 133 | 134 | // Eigen::Vector3d eul = nav::Quat_to_Euler(q); 135 | 136 | // std::cout << ypr << std::endl << std::endl; 137 | // std::cout << eul << std::endl << std::endl; 138 | 139 | // Eigen::Matrix3d dcm = nav::Euler_to_Dcm(ypr); 140 | // Eigen::Matrix3d dcm2 = nav::Quat_to_Dcm(q); 141 | 142 | // Eigen::Vector3d eul2 = nav::Dcm_to_Euler(dcm); 143 | // Eigen::Vector3d eul3 = nav::Dcm_to_Euler(dcm2); 144 | 145 | // std::cout << eul2 << std::endl << std::endl; 146 | // std::cout << eul3 << std::endl << std::endl; 147 | 148 | // Eigen::Quaterniond q2 = nav::Dcm_to_Quat(dcm); 149 | // Eigen::Quaterniond q3 = nav::Dcm_to_Quat(dcm2); 150 | 151 | // // Eigen::Quaterniond q2 = nav::Dcm_to_Quat(dcm); 152 | 153 | // std::cout << q.w() << std::endl; 154 | // std::cout << q.x() << std::endl; 155 | // std::cout << q.y() << std::endl; 156 | // std::cout << q.z() << std::endl << std::endl; 157 | // std::cout << q2.w() << std::endl; 158 | // std::cout << q2.x() << std::endl; 159 | // std::cout << q2.y() << std::endl; 160 | // std::cout << q2.z() << std::endl << std::endl; 161 | // std::cout << q3.w() << std::endl; 162 | // std::cout << q3.x() << std::endl; 163 | // std::cout << q3.y() << std::endl; 164 | // std::cout << q3.z() << std::endl << std::endl; 165 | 166 | } 167 | -------------------------------------------------------------------------------- /img/arduino_logo_75.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bolderflight/navigation/9029646dc2ceb4455c467475743b61dad765240f/img/arduino_logo_75.png -------------------------------------------------------------------------------- /img/logo-words_75.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bolderflight/navigation/9029646dc2ceb4455c467475743b61dad765240f/img/logo-words_75.png -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | angle2dcm KEYWORD2 2 | eul2dcm KEYWORD2 3 | dcm2angle KEYWORD2 4 | dcm2eul KEYWORD2 5 | angle2quat KEYWORD2 6 | eul2quat KEYWORD2 7 | quat2angle KEYWORD2 8 | quat2eul KEYWORD2 9 | quat2dcm KEYWORD2 10 | dcm2quat KEYWORD2 11 | lla2ecef KEYWORD2 12 | ecef2lla KEYWORD2 13 | ecef2ned KEYWORD2 14 | ned2ecef KEYWORD2 15 | lla2ned KEYWORD2 16 | ned2lla KEYWORD2 17 | WrapTo2Pi KEYWORD2 18 | WrapToPi KEYWORD2 19 | Skew KEYWORD2 20 | earthrad_transverse_m KEYWORD2 21 | earthrad_meridonal_m KEYWORD2 22 | earthrad_m KEYWORD2 23 | llarate KEYWORD2 24 | earthrate KEYWORD2 25 | navrate KEYWORD2 -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=Bolder Flight Systems Navigation 2 | version=4.0.2 3 | author=Brian Taylor 4 | maintainer=Brian Taylor 5 | sentence=Navigation and state estimation transformations, utilities, and filters. 6 | paragraph=This is a library of navigation and state estimation transformations, utilities, and filters 7 | category=Data Processing 8 | url=https://github.com/bolderflight/navigation 9 | architectures=* 10 | includes=navigation.h 11 | depends=Bolder Flight Systems Units, Bolder Flight Systems Eigen 12 | -------------------------------------------------------------------------------- /src/constants.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2022 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #ifndef NAVIGATION_SRC_CONSTANTS_H_ // NOLINT 27 | #define NAVIGATION_SRC_CONSTANTS_H_ 28 | 29 | namespace bfs { 30 | /* Semi-major axis, WGS-84 defined, m */ 31 | static constexpr double SEMI_MAJOR_AXIS_LENGTH_M = 6378137.0; 32 | /* Flattening */ 33 | static constexpr double FLATTENING = 1.0 / 298.257223563; 34 | /* Semi-minor axis, m (derived) */ 35 | static constexpr double SEMI_MINOR_AXIS_LENGTH_M = 6356752.3142; 36 | /* First eccentricity (derived) */ 37 | static constexpr double ECC = 8.1819190842622e-2; 38 | /* First eccentricity, squared (derived) */ 39 | static constexpr double ECC2 = 6.69437999014e-3; 40 | /* Angular velocity of the Earth, rad/s */ 41 | static constexpr double WE_RADPS = 7292115.0e-11; 42 | /* Angular velocity of the Earth according to ICD-GPS-200, rad/s */ 43 | static constexpr double WE_GPS_RADPS = 7292115.1467e-11; 44 | /* Earth's Gravitational Constant, m^3/s^2 */ 45 | static constexpr double GM_M3PS2 = 3986004.418e8; 46 | /* Earth's Gravitational Constant according to ICD-GPS-200, m^3/s^2 */ 47 | static constexpr double GM_GPS_M3PS2 = 3986005.0e8; 48 | 49 | } // namespace bfs 50 | 51 | #endif // NAVIGATION_SRC_CONSTANTS_H_ NOLINT 52 | -------------------------------------------------------------------------------- /src/earth_model.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * NavPy developed the original Python algorithms used in this file. Brian 6 | * Taylor converted these algorithms to C++, developed unit tests against 7 | * NavPy generated results, and wrote the documentation. NavPy and Bolder Flight 8 | * Systems Copyrights and licenses are below. 9 | * 10 | * Copyright (c) 2014, NavPy Developers 11 | * All rights reserved. 12 | * 13 | * Redistribution and use in source and binary forms, with or without 14 | * modification, are permitted provided that the following conditions are met: 15 | * 16 | * 1. Redistributions of source code must retain the above copyright notice, this 17 | * list of conditions and the following disclaimer. 18 | * 19 | * 2. Redistributions in binary form must reproduce the above copyright notice, 20 | * this list of conditions and the following disclaimer in the documentation 21 | * and/or other materials provided with the distribution. 22 | * 23 | * 3. Neither the name of the copyright holder nor the names of its contributors 24 | * may be used to endorse or promote products derived from this software 25 | * without specific prior written permission. 26 | * 27 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 28 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 29 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 30 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 31 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 32 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 33 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 35 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 36 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * Copyright (c) 2022 Bolder Flight Systems Inc 39 | * 40 | * Permission is hereby granted, free of charge, to any person obtaining a copy 41 | * of this software and associated documentation files (the “Software”), to 42 | * deal in the Software without restriction, including without limitation the 43 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 44 | * sell copies of the Software, and to permit persons to whom the Software is 45 | * furnished to do so, subject to the following conditions: 46 | * 47 | * The above copyright notice and this permission notice shall be included in 48 | * all copies or substantial portions of the Software. 49 | * 50 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 51 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 52 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 53 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 54 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 55 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 56 | * IN THE SOFTWARE. 57 | */ 58 | 59 | #ifndef NAVIGATION_SRC_EARTH_MODEL_H_ // NOLINT 60 | #define NAVIGATION_SRC_EARTH_MODEL_H_ 61 | 62 | #include 63 | #include 64 | #include "units.h" // NOLINT 65 | #include "constants.h" // NOLINT 66 | #include "eigen.h" // NOLINT 67 | #include "Eigen/Dense" 68 | 69 | namespace bfs { 70 | 71 | /* 72 | * Calculates the radius of curvature in the prime-vertical 73 | * (i.e. transverse or east-west) direction 74 | */ 75 | inline double earthrad_transverse_m(const double lat, 76 | const AngPosUnit ang = AngPosUnit::DEG) { 77 | double rn = SEMI_MAJOR_AXIS_LENGTH_M / std::sqrt(1.0 - (ECC2 * 78 | std::pow(std::sin(convang(lat, ang, AngPosUnit::RAD)), 2.0))); 79 | return rn; 80 | } 81 | 82 | /* 83 | * Calculates the radius of curvature in the meridonal (i.e. north-south) 84 | * direction 85 | */ 86 | inline double earthrad_meridonal_m(const double lat, 87 | const AngPosUnit ang = AngPosUnit::DEG) { 88 | double rm = SEMI_MAJOR_AXIS_LENGTH_M * (1.0 - ECC2) / std::pow(1.0 - ECC2 * 89 | std::pow(std::sin(convang(lat, ang, AngPosUnit::RAD)), 2.0), 90 | 1.5); 91 | return rm; 92 | } 93 | 94 | /* 95 | * Calculates the radius of curvature in the transverse and meridonal directions 96 | */ 97 | inline std::array earthrad_m(const double lat, 98 | const AngPosUnit ang = 99 | AngPosUnit::DEG) { 100 | std::array ret; 101 | ret[0] = earthrad_transverse_m(lat, ang); 102 | ret[1] = earthrad_meridonal_m(lat, ang); 103 | return ret; 104 | } 105 | 106 | /* 107 | * Calculate Latitude, Longitude, Altitude Rate given locally tangent velocity 108 | */ 109 | inline Eigen::Vector3d llarate(const double vn, const double ve, 110 | const double vd, const double lat, 111 | const double alt, 112 | const AngPosUnit ang = AngPosUnit::DEG) { 113 | Eigen::Vector3d lla_dot; 114 | double Rns = earthrad_meridonal_m(lat, ang); 115 | double Rew = earthrad_transverse_m(lat, ang); 116 | lla_dot(0) = convang(vn / (Rns + alt), AngPosUnit::RAD, ang); 117 | lla_dot(1) = convang(ve / (Rew + alt) / 118 | std::cos(convang(lat, ang, AngPosUnit::RAD)), 119 | AngPosUnit::RAD, ang); 120 | lla_dot(2) = -vd; 121 | return lla_dot; 122 | } 123 | 124 | inline Eigen::Vector3d llarate(const Eigen::Vector3d &ned_vel, 125 | const Eigen::Vector3d &lla, 126 | const AngPosUnit ang = AngPosUnit::DEG) { 127 | return llarate(ned_vel(0), ned_vel(1), ned_vel(2), lla(0), lla(2), ang); 128 | } 129 | 130 | /* 131 | * Calculate the earth rotation rate resolved on NED axis given lat 132 | */ 133 | inline Eigen::Vector3d earthrate(const double lat, 134 | const AngPosUnit ang = AngPosUnit::DEG) { 135 | Eigen::Vector3d w = Eigen::Vector3d::Zero(); 136 | w(0) = convang(WE_RADPS * std::cos(convang(lat, ang, AngPosUnit::RAD)), 137 | AngPosUnit::RAD, ang); 138 | w(1) = convang(-WE_RADPS * std::sin(convang(lat, ang, AngPosUnit::RAD)), 139 | AngPosUnit::RAD, ang); 140 | return w; 141 | } 142 | 143 | /* 144 | * Calculate navigation/transport rate given VN, VE, VD, lat, and alt. 145 | * Navigation/transport rate is the angular velocity of the NED frame relative 146 | * to the earth ECEF frame. 147 | */ 148 | inline Eigen::Vector3d navrate(const double vn, const double ve, 149 | const double vd, const double lat, 150 | const double alt, 151 | const AngPosUnit ang = AngPosUnit::DEG) { 152 | Eigen::Vector3d w; 153 | double rew = earthrad_transverse_m(lat, ang); 154 | double rns = earthrad_meridonal_m(lat, ang); 155 | w(0) = convang(ve / (rew + alt), AngPosUnit::RAD, ang); 156 | w(1) = convang(-vn / (rns + alt), AngPosUnit::RAD, ang); 157 | w(2) = convang(-ve * std::tan(convang(lat, ang, AngPosUnit::RAD)) / 158 | (rew + alt), AngPosUnit::RAD, ang); 159 | return w; 160 | } 161 | 162 | inline Eigen::Vector3d navrate(const Eigen::Vector3d &ned_vel, 163 | const Eigen::Vector3d &lla, 164 | const AngPosUnit ang = AngPosUnit::DEG) { 165 | return navrate(ned_vel(0), ned_vel(1), ned_vel(2), lla(0), lla(2), ang); 166 | } 167 | 168 | } // namespace bfs 169 | 170 | #endif // NAVIGATION_SRC_EARTH_MODEL_H_ NOLINT 171 | -------------------------------------------------------------------------------- /src/ekf_15_state.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2022 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #ifndef NAVIGATION_SRC_EKF_15_STATE_H_ // NOLINT 27 | #define NAVIGATION_SRC_EKF_15_STATE_H_ 28 | 29 | #include "constants.h" // NOLINT 30 | #include "earth_model.h" // NOLINT 31 | #include "transforms.h" // NOLINT 32 | #include "utils.h" // NOLINT 33 | #include "units.h" // NOLINT 34 | #include "tilt_compass.h" // NOLINT 35 | #include "eigen.h" // NOLINT 36 | #include "Eigen/Dense" 37 | 38 | namespace bfs { 39 | 40 | class Ekf15State { 41 | public: 42 | Ekf15State() {} 43 | /* Sensor characteristics setters */ 44 | inline void accel_std_mps2(const float val) { 45 | accel_std_mps2_ = val; 46 | } 47 | inline void accel_markov_bias_std_mps2(const float val) { 48 | accel_markov_bias_std_mps2_ = val; 49 | } 50 | inline void accel_tau_s(const float val) { 51 | accel_tau_s_ = val; 52 | } 53 | inline void gyro_std_radps(const float val) { 54 | gyro_std_radps_ = val; 55 | } 56 | inline void gyro_markov_bias_std_radps(const float val) { 57 | gyro_markov_bias_std_radps_ = val; 58 | } 59 | inline void gyro_tau_s(const float val) { 60 | gyro_tau_s_ = val; 61 | } 62 | inline void gnss_pos_ne_std_m(const float val) { 63 | gnss_pos_ne_std_m_ = val; 64 | } 65 | inline void gnss_pos_d_std_m(const float val) { 66 | gnss_pos_d_std_m_ = val; 67 | } 68 | inline void gnss_vel_ne_std_mps(const float val) { 69 | gnss_vel_ne_std_mps_ = val; 70 | } 71 | inline void gnss_vel_d_std_mps(const float val) { 72 | gnss_vel_d_std_mps_ = val; 73 | } 74 | /* Sensor characteristics getters */ 75 | inline float accel_std_mps2() const { 76 | return accel_std_mps2_; 77 | } 78 | inline float accel_markov_bias_std_mps2() const { 79 | return accel_markov_bias_std_mps2_; 80 | } 81 | inline float accel_tau_s() const { 82 | return accel_tau_s_; 83 | } 84 | inline float gyro_std_radps() const { 85 | return gyro_std_radps_; 86 | } 87 | inline float gyro_markov_bias_std_radps() const { 88 | return gyro_markov_bias_std_radps_; 89 | } 90 | inline float gyro_tau_s() const { 91 | return gyro_tau_s_; 92 | } 93 | inline float gnss_pos_ne_std_m() const { 94 | return gnss_pos_ne_std_m_; 95 | } 96 | inline float gnss_pos_d_std_m() const { 97 | return gnss_pos_d_std_m_; 98 | } 99 | inline float gnss_vel_ne_std_mps() const { 100 | return gnss_vel_ne_std_mps_; 101 | } 102 | inline float gnss_vel_d_std_mps() const { 103 | return gnss_vel_d_std_mps_; 104 | } 105 | /* Initial covariance setters */ 106 | inline void init_pos_err_std_m(const float val) { 107 | init_pos_err_std_m_ = val; 108 | } 109 | inline void init_vel_err_std_mps(const float val) { 110 | init_vel_err_std_mps_ = val; 111 | } 112 | inline void init_att_err_std_rad(const float val) { 113 | init_att_err_std_rad_ = val; 114 | } 115 | inline void init_heading_err_std_rad(const float val) { 116 | init_heading_err_std_rad_ = val; 117 | } 118 | inline void init_accel_bias_std_mps2(const float val) { 119 | init_accel_bias_std_mps2_ = val; 120 | } 121 | inline void init_gyro_bias_std_radps(const float val) { 122 | init_gyro_bias_std_radps_ = val; 123 | } 124 | /* Initial covariance getters */ 125 | inline float init_pos_err_std_m() const { 126 | return init_pos_err_std_m_; 127 | } 128 | inline float init_vel_err_std_mps() const { 129 | return init_vel_err_std_mps_; 130 | } 131 | inline float init_att_err_std_rad() const { 132 | return init_att_err_std_rad_; 133 | } 134 | inline float init_heading_err_std_rad() const { 135 | return init_heading_err_std_rad_; 136 | } 137 | inline float init_accel_bias_std_mps2() const { 138 | return init_accel_bias_std_mps2_; 139 | } 140 | inline float init_gyro_bias_std_radps() const { 141 | return init_gyro_bias_std_radps_; 142 | } 143 | /* Initialize the EKF states */ 144 | void Initialize(const Eigen::Vector3f &accel, const Eigen::Vector3f &gyro, 145 | const Eigen::Vector3f &mag, const Eigen::Vector3f &ned_vel, 146 | const Eigen::Vector3d &lla) { 147 | /* Observation matrix */ 148 | h_.block(0, 0, 5, 5) = Eigen::Matrix::Identity(); 149 | /* Process noise covariance */ 150 | rw_.block(0, 0, 3, 3) = accel_std_mps2_ * accel_std_mps2_ * 151 | Eigen::Matrix::Identity(); 152 | rw_.block(3, 3, 3, 3) = gyro_std_radps_ * gyro_std_radps_ * 153 | Eigen::Matrix::Identity(); 154 | rw_.block(6, 6, 3, 3) = 2.0f * accel_markov_bias_std_mps2_ * 155 | accel_markov_bias_std_mps2_ / accel_tau_s_ * 156 | Eigen::Matrix::Identity(); 157 | rw_.block(9, 9, 3, 3) = 2.0f * gyro_markov_bias_std_radps_ * 158 | gyro_markov_bias_std_radps_ / gyro_tau_s_ * 159 | Eigen::Matrix::Identity(); 160 | /* Observation noise covariance */ 161 | r_.block(0, 0, 2, 2) = gnss_pos_ne_std_m_ * gnss_pos_ne_std_m_ * 162 | Eigen::Matrix::Identity(); 163 | r_(2, 2) = gnss_pos_d_std_m_ * gnss_pos_d_std_m_; 164 | r_.block(3, 3, 2, 2) = gnss_vel_ne_std_mps_ * gnss_vel_ne_std_mps_ * 165 | Eigen::Matrix::Identity(); 166 | r_(5, 5) = gnss_vel_d_std_mps_ * gnss_vel_d_std_mps_; 167 | /* Initial covariance estimate */ 168 | p_.block(0, 0, 3, 3) = init_pos_err_std_m_ * init_pos_err_std_m_ * 169 | Eigen::Matrix::Identity(); 170 | p_.block(3, 3, 3, 3) = init_vel_err_std_mps_ * init_vel_err_std_mps_ * 171 | Eigen::Matrix::Identity(); 172 | p_.block(6, 6, 2, 2) = init_att_err_std_rad_ * init_att_err_std_rad_ * 173 | Eigen::Matrix::Identity(); 174 | p_(8, 8) = init_heading_err_std_rad_ * init_heading_err_std_rad_; 175 | p_.block(9, 9, 3, 3) = init_accel_bias_std_mps2_ * 176 | init_accel_bias_std_mps2_ * 177 | Eigen::Matrix::Identity(); 178 | p_.block(12, 12, 3, 3) = init_gyro_bias_std_radps_ * 179 | init_gyro_bias_std_radps_ * 180 | Eigen::Matrix::Identity(); 181 | /* Markov bias matrices */ 182 | accel_markov_bias_ = -1.0f / accel_tau_s_ * 183 | Eigen::Matrix::Identity(); 184 | gyro_markov_bias_ = -1.0f / gyro_tau_s_ * 185 | Eigen::Matrix::Identity(); 186 | /* Initialize position and velocity */ 187 | ins_lla_rad_m_ = lla; 188 | ins_ned_vel_mps_ = ned_vel; 189 | /* Initialize sensor biases */ 190 | gyro_bias_radps_ = gyro; 191 | /* New accelerations and rotation rates */ 192 | ins_accel_mps2_ = accel - accel_bias_mps2_; 193 | ins_gyro_radps_ = gyro - gyro_bias_radps_; 194 | /* Initialize pitch, roll, and heading */ 195 | ins_ypr_rad_ = TiltCompass(accel, mag); 196 | /* Euler to quaternion */ 197 | quat_ = eul2quat(ins_ypr_rad_); 198 | } 199 | /* Perform a time update */ 200 | void TimeUpdate(const Eigen::Vector3f &accel, const Eigen::Vector3f &gyro, 201 | const float dt_s) { 202 | /* A-priori accel and rotation rate estimate */ 203 | ins_accel_mps2_ = accel - accel_bias_mps2_; 204 | ins_gyro_radps_ = gyro - gyro_bias_radps_; 205 | /* Attitude update */ 206 | delta_quat_.w() = 1.0f; 207 | delta_quat_.x() = 0.5f * ins_gyro_radps_(0) * dt_s; 208 | delta_quat_.y() = 0.5f * ins_gyro_radps_(1) * dt_s; 209 | delta_quat_.z() = 0.5f * ins_gyro_radps_(2) * dt_s; 210 | quat_ = (quat_ * delta_quat_).normalized(); 211 | /* Avoid quaternion sign flips */ 212 | if (quat_.w() < 0) { 213 | quat_ = Eigen::Quaternionf(-quat_.w(), -quat_.x(), -quat_.y(), 214 | -quat_.z()); 215 | } 216 | ins_ypr_rad_ = quat2angle(quat_); 217 | /* Body to NED transformation from quat */ 218 | t_b2ned = quat2dcm(quat_).transpose(); 219 | /* Velocity update */ 220 | ins_ned_vel_mps_ += dt_s * (t_b2ned * ins_accel_mps2_ + GRAV_NED_MPS2_); 221 | /* Position update */ 222 | ins_lla_rad_m_ += 223 | (dt_s * llarate(ins_ned_vel_mps_.cast(), 224 | ins_lla_rad_m_)).cast(); 225 | /* Jacobian */ 226 | fs_.block(0, 3, 3, 3) = Eigen::Matrix::Identity(); 227 | fs_(5, 2) = -2.0f * G_MPS2 / SEMI_MAJOR_AXIS_LENGTH_M; 228 | fs_.block(3, 6, 3, 3) = -2.0f * t_b2ned * Skew(ins_accel_mps2_); 229 | fs_.block(3, 9, 3, 3) = -t_b2ned; 230 | fs_.block(6, 6, 3, 3) = -Skew(ins_gyro_radps_); 231 | fs_.block(6, 12, 3, 3) = -0.5f * Eigen::Matrix::Identity(); 232 | fs_.block(9, 9, 3, 3) = accel_markov_bias_; // Accel Markov Bias 233 | fs_.block(12, 12, 3, 3) = gyro_markov_bias_; // Rotation Rate Markov Bias 234 | /* State transition matrix */ 235 | phi_ = Eigen::Matrix::Identity() + fs_ * dt_s; 236 | /* Process Noise Covariance (Discrete approximation) */ 237 | gs_.block(3, 0, 3, 3) = -t_b2ned; 238 | gs_.block(6, 3, 3, 3) = -0.5f * Eigen::Matrix::Identity(); 239 | gs_.block(9, 6, 3, 3) = Eigen::Matrix::Identity(); 240 | gs_.block(12, 9, 3, 3) = Eigen::Matrix::Identity(); 241 | /* Discrete Process Noise */ 242 | q_ = phi_ * dt_s * gs_ * rw_ * gs_.transpose(); 243 | q_ = 0.5f * (q_ + q_.transpose().eval()); 244 | /* Covariance Time Update */ 245 | p_ = phi_ * p_ * phi_.transpose() + q_; 246 | p_ = 0.5f * (p_ + p_.transpose().eval()); 247 | } 248 | /* Perform a measurement update */ 249 | void MeasurementUpdate(const Eigen::Vector3f &ned_vel, 250 | const Eigen::Vector3d &lla) { 251 | /* Y, error between Measures and Outputs */ 252 | y_.segment(0, 3) = lla2ned(lla, ins_lla_rad_m_).cast(); 253 | y_.segment(3, 3) = ned_vel - ins_ned_vel_mps_; 254 | /* Innovation covariance */ 255 | s_ = h_ * p_ * h_.transpose() + r_; 256 | /* Kalman gain */ 257 | k_ = p_ * h_.transpose() * s_.inverse(); 258 | /* Covariance update, P = (I + K * H) * P * (I + K * H)' + K * R * K' */ 259 | p_ = (Eigen::Matrix::Identity() - k_ * h_) * p_ * 260 | (Eigen::Matrix::Identity() - k_ * h_).transpose() 261 | + k_ * r_ * k_.transpose(); 262 | /* State update, x = K * y */ 263 | x_ = k_ * y_; 264 | /* Position update */ 265 | double denom = fabs(1.0 - (ECC2 * sin(ins_lla_rad_m_(0)) * 266 | sin(ins_lla_rad_m_(0)))); 267 | double sqrt_denom = std::sqrt(denom); 268 | double Rns = SEMI_MAJOR_AXIS_LENGTH_M * (1 - ECC2) / 269 | (denom * sqrt_denom); 270 | double Rew = SEMI_MAJOR_AXIS_LENGTH_M / sqrt_denom; 271 | ins_lla_rad_m_(2) -= x_(2); 272 | ins_lla_rad_m_(0) += x_(0) / (Rew + ins_lla_rad_m_(2)); 273 | ins_lla_rad_m_(1) += x_(1) / (Rns + ins_lla_rad_m_(2)) / 274 | cos(ins_lla_rad_m_(0)); 275 | /* Velocity update */ 276 | ins_ned_vel_mps_ += x_.segment(3, 3); 277 | /* Attitude correction */ 278 | delta_quat_.w() = 1.0f; 279 | delta_quat_.x() = x_(6); 280 | delta_quat_.y() = x_(7); 281 | delta_quat_.z() = x_(8); 282 | quat_ = (quat_ * delta_quat_).normalized(); 283 | ins_ypr_rad_ = quat2angle(quat_); 284 | /* Update biases from states */ 285 | accel_bias_mps2_ += x_.segment(9, 3); 286 | gyro_bias_radps_ += x_.segment(12, 3); 287 | /* Update accelerometer and gyro */ 288 | ins_accel_mps2_ -= x_.segment(9, 3); 289 | ins_gyro_radps_ -= x_.segment(12, 3); 290 | } 291 | /* EKF data */ 292 | inline Eigen::Vector3f accel_bias_mps2() const { 293 | return accel_bias_mps2_; 294 | } 295 | inline Eigen::Vector3f gyro_bias_radps() const { 296 | return gyro_bias_radps_; 297 | } 298 | inline Eigen::Vector3f accel_mps2() const { 299 | return ins_accel_mps2_; 300 | } 301 | inline Eigen::Vector3f gyro_radps() const { 302 | return ins_gyro_radps_; 303 | } 304 | inline Eigen::Vector3f ned_vel_mps() const { 305 | return ins_ned_vel_mps_; 306 | } 307 | inline float yaw_rad() const { 308 | return ins_ypr_rad_(0); 309 | } 310 | inline float pitch_rad() const { 311 | return ins_ypr_rad_(1); 312 | } 313 | inline float roll_rad() const { 314 | return ins_ypr_rad_(2); 315 | } 316 | inline Eigen::Vector3d lla_rad_m() const { 317 | return ins_lla_rad_m_; 318 | } 319 | inline double lat_rad() const { 320 | return ins_lla_rad_m_(0); 321 | } 322 | inline double lon_rad() const { 323 | return ins_lla_rad_m_(1); 324 | } 325 | inline double alt_m() const { 326 | return ins_lla_rad_m_(2); 327 | } 328 | 329 | private: 330 | /* 331 | * Sensor characteristics - accel and gyro are modeled with a 332 | * Gauss-Markov model. 333 | */ 334 | /* Standard deviation of accel noise */ 335 | float accel_std_mps2_ = 0.05f; 336 | /* Standard deviation of accel Markov bias */ 337 | float accel_markov_bias_std_mps2_ = 0.01f; 338 | /* Accel correlation time */ 339 | float accel_tau_s_ = 100.0f; 340 | Eigen::Matrix3f accel_markov_bias_ = -1.0f / accel_tau_s_ * 341 | Eigen::Matrix::Identity(); 342 | /* Standard deviation of gyro noise */ 343 | float gyro_std_radps_ = 0.00175f; 344 | /* Standard deviation of gyro Markov bias */ 345 | float gyro_markov_bias_std_radps_ = 0.00025f; 346 | /* Gyro correlation time */ 347 | float gyro_tau_s_ = 50.0f; 348 | Eigen::Matrix3f gyro_markov_bias_ = -1.0f / gyro_tau_s_ * 349 | Eigen::Matrix::Identity(); 350 | /* Standard deviation of the GNSS North and East position measurement */ 351 | float gnss_pos_ne_std_m_ = 3.0f; 352 | /* Standard deviation of the GNSS Down position estimate */ 353 | float gnss_pos_d_std_m_ = 6.0f; 354 | /* Standard deviation of the GNSS North and East velocity measurement */ 355 | float gnss_vel_ne_std_mps_ = 0.5f; 356 | /* Standard deviation of the GNSS Down velocity measurement */ 357 | float gnss_vel_d_std_mps_ = 1.0f; 358 | /* 359 | * Initial set of covariances 360 | */ 361 | /* Standard deviation of the initial position error */ 362 | float init_pos_err_std_m_ = 10.0f; 363 | /* Standard deviation of the initial velocity error */ 364 | float init_vel_err_std_mps_ = 1.0f; 365 | /* Standard deviation of the initial attitude error */ 366 | float init_att_err_std_rad_ = 0.34906f; 367 | /* Standard deviation of the initial heading error */ 368 | float init_heading_err_std_rad_ = 3.14159f; 369 | /* Standard deviation of the initial accel bias */ 370 | float init_accel_bias_std_mps2_ = 0.9810f; 371 | /* Standard deviation of the initial gyro bias */ 372 | float init_gyro_bias_std_radps_ = 0.01745f; 373 | /* 374 | * Kalman filter matrices 375 | */ 376 | /* Observation matrix */ 377 | Eigen::Matrix h_ = Eigen::Matrix::Zero(); 378 | /* Covariance of the observation noise */ 379 | Eigen::Matrix r_ = Eigen::Matrix::Zero(); 380 | /* Covariance of the Sensor Noise */ 381 | Eigen::Matrix rw_ = Eigen::Matrix::Zero(); 382 | /* Process Noise Covariance (Discrete approximation) */ 383 | Eigen::Matrix gs_ = Eigen::Matrix::Zero(); 384 | /* Innovation covariance */ 385 | Eigen::Matrix s_ = Eigen::Matrix::Zero(); 386 | /* Covariance estimate */ 387 | Eigen::Matrix p_ = Eigen::Matrix::Zero(); 388 | /* Discrete Process Noise */ 389 | Eigen::Matrix q_ = Eigen::Matrix::Zero(); 390 | /* Kalman gain */ 391 | Eigen::Matrix k_ = Eigen::Matrix::Zero(); 392 | /* Jacobian (state update matrix) */ 393 | Eigen::Matrix fs_ = Eigen::Matrix::Zero(); 394 | /* State transition */ 395 | Eigen::Matrix phi_ = Eigen::Matrix::Zero(); 396 | /* Error between measures and outputs */ 397 | Eigen::Matrix y_ = Eigen::Matrix::Zero(); 398 | /* State matrix */ 399 | Eigen::Matrix x_ = Eigen::Matrix::Zero(); 400 | /* 401 | * Constants 402 | */ 403 | /* Graviational accel in NED */ 404 | Eigen::Vector3f GRAV_NED_MPS2_ = 405 | (Eigen::Vector3f() << 0.0f, 0.0f, G_MPS2).finished(); 406 | /* 407 | * Intermediates 408 | */ 409 | /* Body to NED transform */ 410 | Eigen::Matrix3f t_b2ned; 411 | /* Acceleration bias, m/s/s */ 412 | Eigen::Vector3f accel_bias_mps2_ = Eigen::Vector3f::Zero(); 413 | /* Rotation rate bias, rad/s */ 414 | Eigen::Vector3f gyro_bias_radps_; 415 | /* Normalized accel */ 416 | Eigen::Vector3f accel_norm_mps2_; 417 | /* Normalized mag */ 418 | Eigen::Vector3f mag_norm_mps2_; 419 | /* Quaternion update */ 420 | Eigen::Quaternionf delta_quat_; 421 | /* Quaternion */ 422 | Eigen::Quaternionf quat_; 423 | /* 424 | * Data 425 | */ 426 | Eigen::Vector3f ins_accel_mps2_; 427 | Eigen::Vector3f ins_gyro_radps_; 428 | Eigen::Vector3f ins_ypr_rad_; 429 | Eigen::Vector3f ins_ned_vel_mps_; 430 | Eigen::Vector3d ins_lla_rad_m_; 431 | }; 432 | 433 | } // namespace bfs 434 | 435 | #endif // NAVIGATION_SRC_EKF_15_STATE_H_ NOLINT 436 | -------------------------------------------------------------------------------- /src/navigation.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2022 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #ifndef NAVIGATION_SRC_NAVIGATION_H_ // NOLINT 27 | #define NAVIGATION_SRC_NAVIGATION_H_ 28 | 29 | #include "constants.h" // NOLINT 30 | #include "ekf_15_state.h" // NOLINT 31 | #include "tilt_compass.h" // NOLINT 32 | #include "earth_model.h" // NOLINT 33 | #include "transforms.h" // NOLINT 34 | #include "utils.h" // NOLINT 35 | 36 | #endif // NAVIGATION_SRC_NAVIGATION_H_ NOLINT 37 | -------------------------------------------------------------------------------- /src/tilt_compass.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2022 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | #ifndef NAVIGATION_SRC_TILT_COMPASS_H_ // NOLINT 26 | #define NAVIGATION_SRC_TILT_COMPASS_H_ 27 | 28 | #include "eigen.h" // NOLINT 29 | #include "Eigen/Dense" 30 | 31 | namespace bfs { 32 | /* 33 | * Yaw, pitch, and roll from 3-axis accelerometer and 34 | * 3-axis magnetometer measurements 35 | */ 36 | inline Eigen::Vector3f TiltCompass(const Eigen::Vector3f &accel, 37 | const Eigen::Vector3f &mag) { 38 | Eigen::Vector3f ypr; 39 | Eigen::Vector3f a = accel; 40 | Eigen::Vector3f m = mag; 41 | /* Normalize accel and mag */ 42 | a.normalize(); 43 | m.normalize(); 44 | /* Pitch */ 45 | ypr(1) = std::asin(a(0)); 46 | /* Roll */ 47 | ypr(2) = std::asin(-a(1) / std::cos(ypr(1))); 48 | /* Yaw */ 49 | ypr(0) = std::atan2(m(2) * std::sin(ypr(2)) - m(1) * std::cos(ypr(2)), 50 | m(0) * std::cos(ypr(1)) + m(1) * std::sin(ypr(1)) * std::sin(ypr(2)) 51 | + m(2) * std::sin(ypr(1)) * std::cos(ypr(2))); 52 | return ypr; 53 | } 54 | 55 | } // namespace bfs 56 | 57 | #endif // NAVIGATION_SRC_TILT_COMPASS_H_ NOLINT 58 | -------------------------------------------------------------------------------- /src/transforms.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2022 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #ifndef NAVIGATION_SRC_TRANSFORMS_H_ // NOLINT 27 | #define NAVIGATION_SRC_TRANSFORMS_H_ 28 | 29 | #include "units.h" // NOLINT 30 | #include "eigen.h" // NOLINT 31 | #include "Eigen/Dense" 32 | 33 | namespace bfs { 34 | /* Convert rotation angles to direction cosine matrix */ 35 | template 36 | Eigen::Matrix angle2dcm(const T rot1, const T rot2, const T rot3, 37 | const AngPosUnit ang = AngPosUnit::RAD) { 38 | static_assert(std::is_floating_point::value, 39 | "Only floating point types supported"); 40 | Eigen::Matrix dcm; 41 | T cos_rot1 = std::cos(convang(rot1, ang, AngPosUnit::RAD)); 42 | T sin_rot1 = std::sin(convang(rot1, ang, AngPosUnit::RAD)); 43 | T cos_rot2 = std::cos(convang(rot2, ang, AngPosUnit::RAD)); 44 | T sin_rot2 = std::sin(convang(rot2, ang, AngPosUnit::RAD)); 45 | T cos_rot3 = std::cos(convang(rot3, ang, AngPosUnit::RAD)); 46 | T sin_rot3 = std::sin(convang(rot3, ang, AngPosUnit::RAD)); 47 | dcm(0, 0) = cos_rot2 * cos_rot1; 48 | dcm(1, 0) = -cos_rot3 * sin_rot1 + sin_rot3 * sin_rot2 * cos_rot1; 49 | dcm(2, 0) = sin_rot3 * sin_rot1 + cos_rot3 * sin_rot2 * cos_rot1; 50 | dcm(0, 1) = cos_rot2 * sin_rot1; 51 | dcm(1, 1) = cos_rot3 * cos_rot1 + sin_rot3 * sin_rot2 * sin_rot1; 52 | dcm(2, 1) = -sin_rot3 * cos_rot1 + cos_rot3 * sin_rot2 * sin_rot1; 53 | dcm(0, 2) = -sin_rot2; 54 | dcm(1, 2) = sin_rot3 * cos_rot2; 55 | dcm(2, 2) = cos_rot3 * cos_rot2; 56 | return dcm; 57 | } 58 | /* Convert Euler angles to direction cosine matrix */ 59 | template 60 | Eigen::Matrix eul2dcm(const Eigen::Matrix &eul, 61 | const AngPosUnit ang = AngPosUnit::RAD) { 62 | return angle2dcm(eul(0), eul(1), eul(2), ang); 63 | } 64 | /* Create rotation angles from direction cosine matrix*/ 65 | template 66 | Eigen::Matrix dcm2angle(const Eigen::Matrix &dcm, 67 | const AngPosUnit ang = AngPosUnit::RAD) { 68 | static_assert(std::is_floating_point::value, 69 | "Only floating point types supported"); 70 | Eigen::Matrix an; 71 | an(0, 0) = convang(std::atan2(dcm(0, 1), dcm(0, 0)), AngPosUnit::RAD, ang); 72 | an(1, 0) = convang(-std::asin(dcm(0, 2)), AngPosUnit::RAD, ang); 73 | an(2, 0) = convang(std::atan2(dcm(1, 2), dcm(2, 2)), AngPosUnit::RAD, ang); 74 | return an; 75 | } 76 | /* Create Euler angles from direction cosine matrix */ 77 | template 78 | Eigen::Matrix dcm2eul(const Eigen::Matrix &dcm, 79 | const AngPosUnit ang = AngPosUnit::RAD) { 80 | return dcm2angle(dcm, ang); 81 | } 82 | /* Convert rotation angles to quaternion */ 83 | template 84 | Eigen::Quaternion angle2quat(const T rot1, const T rot2, const T rot3, 85 | const AngPosUnit a = AngPosUnit::RAD) { 86 | static_assert(std::is_floating_point::value, 87 | "Only floating point types supported"); 88 | Eigen::Quaternion q; 89 | T cos_rot1 = std::cos(convang(rot1 / static_cast(2), a, AngPosUnit::RAD)); 90 | T sin_rot1 = std::sin(convang(rot1 / static_cast(2), a, AngPosUnit::RAD)); 91 | T cos_rot2 = std::cos(convang(rot2 / static_cast(2), a, AngPosUnit::RAD)); 92 | T sin_rot2 = std::sin(convang(rot2 / static_cast(2), a, AngPosUnit::RAD)); 93 | T cos_rot3 = std::cos(convang(rot3 / static_cast(2), a, AngPosUnit::RAD)); 94 | T sin_rot3 = std::sin(convang(rot3 / static_cast(2), a, AngPosUnit::RAD)); 95 | q.w() = cos_rot1 * cos_rot2 * cos_rot3 + sin_rot1 * sin_rot2 * sin_rot3; 96 | q.x() = cos_rot1 * cos_rot2 * sin_rot3 - sin_rot1 * sin_rot2 * cos_rot3; 97 | q.y() = cos_rot1 * sin_rot2 * cos_rot3 + sin_rot1 * cos_rot2 * sin_rot3; 98 | q.z() = sin_rot1 * cos_rot2 * cos_rot3 - cos_rot1 * sin_rot2 * sin_rot3; 99 | return q; 100 | } 101 | /* Convert Euler angles to quaternion */ 102 | template 103 | Eigen::Quaternion eul2quat(const Eigen::Matrix &eul, 104 | const AngPosUnit a = AngPosUnit::RAD) { 105 | return angle2quat(eul(0), eul(1), eul(2), a); 106 | } 107 | /* Convert quaternion to rotation angles */ 108 | template 109 | Eigen::Matrix quat2angle(const Eigen::Quaternion &q, 110 | const AngPosUnit ang = AngPosUnit::RAD) { 111 | static_assert(std::is_floating_point::value, 112 | "Only floating point types supported"); 113 | Eigen::Matrix angle; 114 | T m11 = static_cast(2) * q.w() * q.w() + static_cast(2) * q.x() * 115 | q.x() - static_cast(1); 116 | T m12 = static_cast(2) * q.x() * q.y() + static_cast(2) * q.w() * 117 | q.z(); 118 | T m13 = static_cast(2) * q.x() * q.z() - static_cast(2) * q.w() * 119 | q.y(); 120 | T m23 = static_cast(2) * q.y() * q.z() + static_cast(2) * q.w() * 121 | q.x(); 122 | T m33 = static_cast(2) * q.w() * q.w() + static_cast(2) * q.z() * 123 | q.z() - static_cast(1); 124 | angle(0, 0) = convang(std::atan2(m12, m11), AngPosUnit::RAD, ang); 125 | angle(1, 0) = convang(std::asin(-m13), AngPosUnit::RAD, ang); 126 | angle(2, 0) = convang(std::atan2(m23, m33), AngPosUnit::RAD, ang); 127 | return angle; 128 | } 129 | /* Convert quaternion to Euler angles */ 130 | template 131 | Eigen::Matrix quat2eul(const Eigen::Quaternion &q, 132 | const AngPosUnit ang = AngPosUnit::RAD) { 133 | return quat2angle(q, ang); 134 | } 135 | 136 | /* Convert direction cosine matrix to quaternion */ 137 | template 138 | Eigen::Quaternion dcm2quat(const Eigen::Matrix &dcm) { 139 | static_assert(std::is_floating_point::value, 140 | "Only floating point types supported"); 141 | Eigen::Quaternion q; 142 | q.w() = static_cast(0.5) * std::sqrt(static_cast(1) + dcm(0, 0) + 143 | dcm(1, 1) + dcm(2, 2)); 144 | q.x() = (dcm(1, 2) - dcm(2, 1)) / (static_cast(4) * q.w()); 145 | q.y() = (dcm(2, 0) - dcm(0, 2)) / (static_cast(4) * q.w()); 146 | q.z() = (dcm(0, 1) - dcm(1, 0)) / (static_cast(4) * q.w()); 147 | return q; 148 | } 149 | /* Convert quaternion to direction cosine matrix */ 150 | template 151 | Eigen::Matrix quat2dcm(const Eigen::Quaternion &q) { 152 | static_assert(std::is_floating_point::value, 153 | "Only floating point types supported"); 154 | Eigen::Matrix dcm; 155 | dcm(0, 0) = static_cast(2) * q.w() * q.w() - static_cast(1) + 156 | static_cast(2) * q.x() * q.x(); 157 | dcm(1, 0) = static_cast(2) * q.x() * q.y() - static_cast(2) * 158 | q.w() * q.z(); 159 | dcm(2, 0) = static_cast(2) * q.x() * q.z() + static_cast(2) * 160 | q.w() * q.y(); 161 | dcm(0, 1) = static_cast(2) * q.x() * q.y() + static_cast(2) * 162 | q.w() * q.z(); 163 | dcm(1, 1) = static_cast(2) * q.w() * q.w() - static_cast(1) + 164 | static_cast(2) * q.y() * q.y(); 165 | dcm(2, 1) = static_cast(2) * q.y() * q.z() - static_cast(2) * 166 | q.w() * q.x(); 167 | dcm(0, 2) = static_cast(2) * q.x() * q.z() - static_cast(2) * 168 | q.w() * q.y(); 169 | dcm(1, 2) = static_cast(2) * q.y() * q.z() + static_cast(2) * 170 | q.w() * q.x(); 171 | dcm(2, 2) = static_cast(2) * q.w() * q.w() - static_cast(1) + 172 | static_cast(2) * q.z() * q.z(); 173 | return dcm; 174 | } 175 | 176 | /* 177 | * Convert geodetic coordinates to Earth-centered Earth-fixed (ECEF) coordinates 178 | */ 179 | inline Eigen::Vector3d lla2ecef(const Eigen::Vector3d &lla, 180 | const AngPosUnit ang = AngPosUnit::DEG) { 181 | Eigen::Vector3d ecef; 182 | double sin_lat = std::sin(convang(lla(0), ang, AngPosUnit::RAD)); 183 | double cos_lat = std::cos(convang(lla(0), ang, AngPosUnit::RAD)); 184 | double cos_lon = std::cos(convang(lla(1), ang, AngPosUnit::RAD)); 185 | double sin_lon = std::sin(convang(lla(1), ang, AngPosUnit::RAD)); 186 | double alt = lla(2); 187 | double Rn = SEMI_MAJOR_AXIS_LENGTH_M / 188 | std::sqrt(std::fabs(1.0 - (ECC2 * sin_lat * sin_lat))); 189 | ecef(0) = (Rn + alt) * cos_lat * cos_lon; 190 | ecef(1) = (Rn + alt) * cos_lat * sin_lon; 191 | ecef(2) = (Rn * (1.0 - ECC2) + alt) * sin_lat; 192 | return ecef; 193 | } 194 | 195 | /* 196 | * Convert Earth-centered Earth-fixed (ECEF) coordinates to geodetic coordinates, 197 | * using Olson's method 198 | */ 199 | inline Eigen::Vector3d ecef2lla(const Eigen::Vector3d &ecef, 200 | const AngPosUnit ang = AngPosUnit::DEG) { 201 | Eigen::Vector3d lla = Eigen::Vector3d::Zero(); 202 | static constexpr double A1 = SEMI_MAJOR_AXIS_LENGTH_M * ECC2; 203 | static constexpr double A2 = A1 * A1; 204 | static constexpr double A3 = A1 * ECC2 / 2.0; 205 | static constexpr double A4 = 2.5 * A2; 206 | static constexpr double A5 = A1 + A3; 207 | static constexpr double A6 = 1.0 - ECC2; 208 | static double x, y, z, zp, w2, w, z2, r2, r, s2, c2, u, 209 | v, s, ss, c, g, rg, rf, f, m, p; 210 | x = ecef(0); 211 | y = ecef(1); 212 | z = ecef(2); 213 | zp = std::fabs(z); 214 | w2 = x * x + y * y; 215 | w = std::sqrt(w2); 216 | z2 = z * z; 217 | r2 = w2 + z2; 218 | r = std::sqrt(r2); 219 | if (r < 100000.0) { 220 | return lla; 221 | } 222 | lla(1) = std::atan2(y, x); 223 | s2 = z2 / r2; 224 | c2 = w2 / r2; 225 | u = A2 / r; 226 | v = A3 - A4 / r; 227 | if (c2 > 0.3) { 228 | s = (zp / r) * (1.0 + c2 * (A1 + u + s2 * v) / r); 229 | lla(0) = std::asin(s); 230 | ss = s * s; 231 | c = std::sqrt(1.0 - ss); 232 | } else { 233 | c = (w / r) * (1.0 - s2 * (A5 - u - c2 * v) / r); 234 | lla(0) = std::acos(c); 235 | ss = 1.0 - c * c; 236 | s = std::sqrt(ss); 237 | } 238 | g = 1.0 - ECC2 * ss; 239 | rg = SEMI_MAJOR_AXIS_LENGTH_M / std::sqrt(g); 240 | rf = A6 * rg; 241 | u = w - rg * c; 242 | v = zp - rf * s; 243 | f = c * u + s * v; 244 | m = c * v - s * u; 245 | p = m / (rf / g + f); 246 | lla(0) = lla(0) + p; 247 | lla(2) = f + m * p / 2.0; 248 | if (z < 0) { 249 | lla(0) = -1 * lla(0); 250 | } 251 | lla(0) = convang(lla(0), AngPosUnit::RAD, ang); 252 | lla(1) = convang(lla(1), AngPosUnit::RAD, ang); 253 | return lla; 254 | } 255 | 256 | /* 257 | * Transform geocentric Earth-centered Earth-fixed coordinates to 258 | * local north-east-down 259 | */ 260 | inline Eigen::Vector3d ecef2ned(const Eigen::Vector3d &ecef, 261 | const Eigen::Vector3d &lla0, 262 | const AngPosUnit ang = AngPosUnit::DEG) { 263 | Eigen::Matrix3d R; 264 | double sin_lat = std::sin(convang(lla0(0), ang, AngPosUnit::RAD)); 265 | double cos_lat = std::cos(convang(lla0(0), ang, AngPosUnit::RAD)); 266 | double sin_lon = std::sin(convang(lla0(1), ang, AngPosUnit::RAD)); 267 | double cos_lon = std::cos(convang(lla0(1), ang, AngPosUnit::RAD)); 268 | R(0, 0) = -sin_lat * cos_lon; 269 | R(0, 1) = -sin_lat * sin_lon; 270 | R(0, 2) = cos_lat; 271 | R(1, 0) = -sin_lon; 272 | R(1, 1) = cos_lon; 273 | R(1, 2) = 0; 274 | R(2, 0) = -cos_lat * cos_lon; 275 | R(2, 1) = -cos_lat * sin_lon; 276 | R(2, 2) = -sin_lat; 277 | return R * ecef; 278 | } 279 | 280 | /* 281 | * Transform a vector resolved in NED (origin given by lat_ref, lon_ref, 282 | * and alt_ref) coordinates to its ECEF representation. 283 | */ 284 | inline Eigen::Vector3d ned2ecef(const Eigen::Vector3d &ned, 285 | const Eigen::Vector3d &lla0, 286 | const AngPosUnit ang = AngPosUnit::DEG) { 287 | Eigen::Matrix3d R; 288 | double sin_lat = std::sin(convang(lla0(0), ang, AngPosUnit::RAD)); 289 | double cos_lat = std::cos(convang(lla0(0), ang, AngPosUnit::RAD)); 290 | double sin_lon = std::sin(convang(lla0(1), ang, AngPosUnit::RAD)); 291 | double cos_lon = std::cos(convang(lla0(1), ang, AngPosUnit::RAD)); 292 | R(0, 0) = -sin_lat * cos_lon; 293 | R(0, 1) = -sin_lat * sin_lon; 294 | R(0, 2) = cos_lat; 295 | R(1, 0) = -sin_lon; 296 | R(1, 1) = cos_lon; 297 | R(1, 2) = 0; 298 | R(2, 0) = -cos_lat * cos_lon; 299 | R(2, 1) = -cos_lat * sin_lon; 300 | R(2, 2) = -sin_lat; 301 | return R.transpose() * ned; 302 | } 303 | 304 | /* Transform geodetic coordinates to local north-east-down coordinates */ 305 | inline Eigen::Vector3d lla2ned(const Eigen::Vector3d &lla, 306 | const Eigen::Vector3d &lla0, 307 | const AngPosUnit ang = AngPosUnit::DEG) { 308 | Eigen::Vector3d loc = lla2ecef(lla, ang); 309 | Eigen::Vector3d ref = lla2ecef(lla0, ang); 310 | return ecef2ned(loc - ref, lla0, ang); 311 | } 312 | 313 | /* Transform local north-east-down coordinates to geodetic coordinates */ 314 | inline Eigen::Vector3d ned2lla(const Eigen::Vector3d &ned, 315 | const Eigen::Vector3d &lla0, 316 | const AngPosUnit ang = AngPosUnit::DEG) { 317 | Eigen::Vector3d loc = ned2ecef(ned, lla0, ang); 318 | Eigen::Vector3d ref = lla2ecef(lla0, ang); 319 | return ecef2lla(loc + ref, ang); 320 | } 321 | 322 | } // namespace bfs 323 | 324 | #endif // NAVIGATION_SRC_TRANSFORMS_H_ NOLINT 325 | -------------------------------------------------------------------------------- /src/utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Brian R Taylor 3 | * brian.taylor@bolderflight.com 4 | * 5 | * Copyright (c) 2022 Bolder Flight Systems Inc 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the “Software”), to 9 | * deal in the Software without restriction, including without limitation the 10 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | * sell copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in 15 | * all copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | * IN THE SOFTWARE. 24 | */ 25 | 26 | #ifndef NAVIGATION_SRC_UTILS_H_ // NOLINT 27 | #define NAVIGATION_SRC_UTILS_H_ 28 | 29 | #include "eigen.h" // NOLINT 30 | #include "Eigen/Dense" 31 | 32 | namespace bfs { 33 | 34 | /* Converts a +/- 180 value to a 0 - 360 value */ 35 | template 36 | T WrapTo2Pi(T ang) { 37 | static_assert(std::is_floating_point::value, 38 | "Only floating point types supported"); 39 | ang = std::fmod(ang, BFS_2PI); 40 | if (ang < static_cast(0)) { 41 | ang += BFS_2PI; 42 | } 43 | return ang; 44 | } 45 | 46 | /* Converts a 0 - 360 value to a +/- 180 value */ 47 | template 48 | T WrapToPi(T ang) { 49 | static_assert(std::is_floating_point::value, 50 | "Only floating point types supported"); 51 | if (ang > BFS_PI) { 52 | ang -= BFS_2PI; 53 | } 54 | if (ang < -BFS_PI) { 55 | ang += BFS_2PI; 56 | } 57 | return ang; 58 | } 59 | 60 | /* Skew symmetric matrix from a given vector w */ 61 | template 62 | Eigen::Matrix Skew(const Eigen::Matrix &w) { 63 | Eigen::Matrix c; 64 | c(0, 0) = 0.0; 65 | c(0, 1) = -w(2); 66 | c(0, 2) = w(1); 67 | c(1, 0) = w(2); 68 | c(1, 1) = 0.0; 69 | c(1, 2) = -w(0); 70 | c(2, 0) = -w(1); 71 | c(2, 1) = w(0); 72 | c(2, 2) = 0.0; 73 | return c; 74 | } 75 | 76 | } // namespace bfs 77 | 78 | #endif // NAVIGATION_SRC_UTILS_H_ NOLINT 79 | --------------------------------------------------------------------------------