├── .github └── workflows │ └── cmake.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── benchmark ├── BenchmarkMain.cpp ├── CMakeLists.txt ├── CoreBenchmark.cpp ├── MeasurementVectorBenchmark.cpp ├── SquareRootCoreBenchmark.cpp └── StateVectorBenchmark.cpp ├── examples ├── CMakeLists.txt ├── ahrs │ ├── CMakeLists.txt │ ├── ahrs.cpp │ ├── ahrs.h │ └── python │ │ └── ukf │ │ └── __init__.py └── sfwa_ukf │ ├── CMakeLists.txt │ ├── cukf.cpp │ ├── cukf.h │ └── python │ └── ukf │ └── __init__.py ├── include └── UKF │ ├── Config.h │ ├── Core.h │ ├── Integrator.h │ ├── MeasurementVector.h │ ├── StateVector.h │ └── Types.h ├── patch └── eigen-3.2.8-no-malloc-in-llt.patch └── test ├── CMakeLists.txt ├── TestCore.cpp ├── TestDynamicMeasurementVector.cpp ├── TestFixedMeasurementVector.cpp ├── TestSquareRootCore.cpp ├── TestStateVector.cpp └── comparisons.h /.github/workflows/cmake.yml: -------------------------------------------------------------------------------- 1 | name: CMake 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | pull_request: 8 | 9 | env: 10 | # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) 11 | BUILD_TYPE: Release 12 | 13 | jobs: 14 | build: 15 | # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. 16 | # You can convert this to a matrix build if you need cross-platform coverage. 17 | # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix 18 | runs-on: ${{ matrix.config.os }} 19 | strategy: 20 | fail-fast: false 21 | matrix: 22 | config: 23 | - { os: ubuntu-18.04, CC: gcc-7, CXX: g++-7 } 24 | - { os: ubuntu-18.04, CC: clang-5.0, CXX: clang++-5.0 } 25 | - { os: ubuntu-18.04, CC: clang-9, CXX: clang++-9 } 26 | - { os: ubuntu-20.04, CC: gcc-7, CXX: g++-7 } 27 | - { os: ubuntu-20.04, CC: clang-8, CXX: clang++-8 } 28 | - { os: ubuntu-20.04, CC: gcc-10, CXX: g++-10 } 29 | - { os: ubuntu-20.04, CC: clang-12, CXX: clang++-12 } 30 | - { os: macOS-latest, CC: clang, CXX: clang++ } 31 | # - { os: windows-latest, CC: gcc, CXX: g++ } 32 | 33 | steps: 34 | - uses: actions/checkout@v3 35 | 36 | - name: Install compiler 37 | run: | 38 | if [ "$RUNNER_OS" == "Linux" ]; then 39 | sudo apt-get update && sudo apt-get install ${{ matrix.config.CC }} ${{ matrix.config.CXX }} 40 | fi 41 | shell: bash 42 | 43 | - name: Configure CMake 44 | env: 45 | CC: ${{ matrix.config.CC }} 46 | CXX: ${{ matrix.config.CXX }} 47 | # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. 48 | # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type 49 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} 50 | 51 | - name: Build 52 | env: 53 | CC: ${{ matrix.config.CC }} 54 | CXX: ${{ matrix.config.CXX }} 55 | # Build your program with the given configuration 56 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 57 | 58 | - name: Test 59 | env: 60 | CC: ${{ matrix.config.CC }} 61 | CXX: ${{ matrix.config.CXX }} 62 | # Execute tests defined by the CMake configuration. 63 | # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail 64 | run: cd ${{github.workspace}}/build && make unittest && test/unittest 65 | 66 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.py[cod] 2 | .DS_Store 3 | 4 | # C extensions 5 | *.so 6 | *.dylib 7 | *.a 8 | a.out 9 | 10 | # Packages 11 | *.egg 12 | *.egg-info 13 | dist 14 | build 15 | eggs 16 | parts 17 | var 18 | sdist 19 | develop-eggs 20 | .installed.cfg 21 | lib64 22 | Debug 23 | Release 24 | 25 | # Installer logs 26 | pip-log.txt 27 | 28 | # Unit test / coverage reports 29 | .coverage 30 | .tox 31 | nosetests.xml 32 | 33 | # Translations 34 | *.mo 35 | 36 | # Mr Developer 37 | .mr.developer.cfg 38 | .pydevproject 39 | 40 | # LaTeX 41 | *.synctex.gz 42 | *.aux 43 | *.log 44 | 45 | # Symlink to dynamic library for development 46 | python/ukf/c 47 | python/ukf/ccs-c66x 48 | 49 | # Cmake junk 50 | CMakeFiles 51 | *.cmake 52 | CMakeCache.txt 53 | src/googletest 54 | src/eigen3* 55 | src/*.gz 56 | Makefile 57 | test/src/googletest* 58 | tmp 59 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8.4 FATAL_ERROR) 2 | PROJECT(ukf) 3 | INCLUDE(ExternalProject) 4 | INCLUDE(CheckCXXCompilerFlag) 5 | 6 | # Check for C++14 support. 7 | CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) 8 | IF(COMPILER_SUPPORTS_CXX14) 9 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") 10 | ELSE() 11 | MESSAGE(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.") 12 | ENDIF() 13 | 14 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-register -Wno-ignored-attributes") 15 | 16 | # Set default ExternalProject root directory 17 | SET_DIRECTORY_PROPERTIES(PROPERTIES EP_PREFIX .) 18 | 19 | # Add eigen3.2.8 20 | ExternalProject_Add( 21 | eigen3 22 | URL https://gitlab.com/libeigen/eigen/-/archive/3.2.8/eigen-3.2.8.tar.gz 23 | TIMEOUT 30 24 | # Apply the LLT malloc fix patch, necessary to avoid dynamic memory allocation on embedded systems. 25 | # This can be omitted on systems where dynamic memory allocation is acceptable. 26 | PATCH_COMMAND patch -p1 -t -N < ${PROJECT_SOURCE_DIR}/patch/eigen-3.2.8-no-malloc-in-llt.patch 27 | # Disable install step 28 | INSTALL_COMMAND "" 29 | # Wrap download, configure and build steps in a script to log output 30 | LOG_DOWNLOAD ON 31 | LOG_CONFIGURE ON 32 | LOG_BUILD ON) 33 | 34 | ExternalProject_Get_Property(eigen3 source_dir) 35 | SET(eigen_dir ${source_dir}) 36 | 37 | INCLUDE_DIRECTORIES(${eigen_dir}) 38 | 39 | GET_DIRECTORY_PROPERTY(hasParent PARENT_DIRECTORY) 40 | IF(hasParent) 41 | SET(eigen_dir ${eigen_dir} PARENT_SCOPE) 42 | ENDIF() 43 | 44 | ENABLE_TESTING() 45 | 46 | ADD_SUBDIRECTORY(test EXCLUDE_FROM_ALL) 47 | ADD_SUBDIRECTORY(benchmark EXCLUDE_FROM_ALL) 48 | ADD_SUBDIRECTORY(examples EXCLUDE_FROM_ALL) 49 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (C) 2016 Thiemar Pty Ltd 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in 11 | all copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. 20 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # UKF 2 | 3 | Unscented Kalman filter library. Several different UKF implementations are 4 | provided: 5 | 6 | * Standard Unscented Kalman Filter for state estimation, as originally 7 | described in [[1]](#ref1), with extensions for quaternions as described 8 | in [[2]](#ref2). 9 | * Square-root Unscented Kalman Filter for state estimation, implemented as 10 | described in [[3]](#ref3). 11 | * Optimised form of square-root Unscented Kalman filter for parameter 12 | estimation, implemented as described in [[3]](#ref3). 13 | 14 | This library makes use of the [Eigen](https://eigen.tuxfamily.org) library 15 | for linear algebra routines and matrix and vector operations. Heavy use is 16 | made of C++11 and C++14 features in an attempt to avoid any dynamic memory 17 | allocations and maximise opportunities for compile-time optimisations. 18 | 19 | A primary goal of the library is to provide efficient UKF implementations for 20 | use on embedded systems, so there is a strong focus on having minimal 21 | dependencies and avoiding non-deterministic operations. 22 | 23 | The filter can be compiled using either single or double precision by 24 | choosing one of the following preprocessor definitions: 25 | 26 | * UKF_SINGLE_PRECISION 27 | * UKF_DOUBLE_PRECISION 28 | 29 | ## Usage 30 | 31 | The library contains a number of class templates which are to be specialised 32 | for the particular application. There are three main classes which need to be 33 | specialised to make up an implementation: 34 | 35 | * State vector 36 | * Measurement vector 37 | * Core 38 | 39 | The following examples are all derived from the [unit tests](test/), so have 40 | a look at them for more detail. 41 | 42 | ### State vector 43 | 44 | The state vector and measurement vector are made up of a number of fields, 45 | each of which contains a key and a type. The reason for this is to allow 46 | the filter to handle quaternions in the state vector transparently, so 47 | something like the following is allowed: 48 | 49 | ```C++ 50 | enum MyFields { 51 | LatLon, 52 | Altitude, 53 | Velocity, 54 | Attitude 55 | }; 56 | 57 | using MyStateVector = UKF::StateVector< 58 | UKF::Field>, 59 | UKF::Field, 60 | UKF::Field>, 61 | UKF::Field 62 | >; 63 | ``` 64 | 65 | Internally, these fields are all stored together as one contiguous Eigen 66 | column vector, and all key lookups are done at compile time. 67 | 68 | UKF scaling parameters can be adjusted in the following way: 69 | 70 | ```C++ 71 | template <> constexpr real_t UKF::Parameters::AlphaSquared = 1e-6; 72 | ``` 73 | 74 | For a description of what the scaling parameters do, see [[2]](#ref2) or read 75 | the comments in the [code](include/UKF/StateVector.h). 76 | 77 | ### Measurement vector 78 | 79 | The measurement vector can be specialised in a similar way, but with the 80 | choice of a fixed or dynamic measurement vector: 81 | 82 | ```C++ 83 | enum MyFields { 84 | StaticPressure, 85 | DynamicPressure, 86 | Accelerometer, 87 | Gyroscope 88 | }; 89 | 90 | using MyMeasurementVector = UKF::FixedMeasurementVector< 91 | UKF::Field>, 92 | UKF::Field>, 93 | UKF::Field, 94 | UKF::Field 95 | >; 96 | ``` 97 | 98 | or: 99 | 100 | ```C++ 101 | enum MyFields { 102 | StaticPressure, 103 | DynamicPressure, 104 | Accelerometer, 105 | Gyroscope 106 | }; 107 | 108 | using MyMeasurementVector = UKF::DynamicMeasurementVector< 109 | UKF::Field>, 110 | UKF::Field>, 111 | UKF::Field, 112 | UKF::Field 113 | >; 114 | ``` 115 | 116 | For the fixed measurement vector, all measurements have to be provided every 117 | filter iteration. The dynamic measurement vector allows for a filter where 118 | some measurements are not available at every iteration, and so should only be 119 | fused when they are available. 120 | 121 | There is a small performance overhead for the dynamic measurement vector, 122 | but it does not do any dynamic memory allocation. 123 | 124 | ### Core 125 | 126 | The Core class contains the filter state and step function. It can be 127 | specialised as follows: 128 | 129 | ```C++ 130 | using MyCore = UKF::Core< 131 | MyStateVector, 132 | MyMeasurementVector, 133 | UKF::IntegratorRK4 134 | >; 135 | ``` 136 | 137 | Here, the user-specialised state vector and measurement vector classes are 138 | provided as template parameters, along with the integrator to use for the 139 | process model. For a list of available integrators, see 140 | [Integrator.h](include/UKF/Integrator.h). 141 | 142 | The square-root state estimation filter can be used instead like this: 143 | 144 | ```C++ 145 | using MyCore = UKF::SquareRootCore< 146 | MyStateVector, 147 | MyMeasurementVector, 148 | UKF::IntegratorRK4 149 | >; 150 | ``` 151 | 152 | Specialisations of the process model (for state estimation filters) and 153 | measurement model must also be provided. 154 | 155 | ### Process model 156 | 157 | The process model is implemented as an ODE, with a user-provided function to 158 | calculate the derivative. The ODE is then solved using the integrator method 159 | specified in the Core class specialisation. 160 | 161 | Here is an example of a process model for a simple state vector: 162 | 163 | ```C++ 164 | using ProcessModelTestStateVector = UKF::StateVector< 165 | UKF::Field>, 166 | UKF::Field> 167 | >; 168 | 169 | template <> template <> 170 | ProcessModelTestStateVector ProcessModelTestStateVector::derivative<>() const { 171 | ProcessModelTestStateVector temp; 172 | /* Position derivative. */ 173 | temp.set_field(get_field()); 174 | 175 | /* Velocity derivative. */ 176 | temp.set_field(UKF::Vector<3>(0, 0, 0)); 177 | 178 | return temp; 179 | } 180 | ``` 181 | 182 | Also, the process model can take an arbitrary number of user-specified 183 | inputs, like this: 184 | 185 | ```C++ 186 | template <> template <> 187 | ProcessModelTestStateVector ProcessModelTestStateVector::derivative>( 188 | const UKF::Vector<3>& acceleration) const { 189 | ProcessModelTestStateVector temp; 190 | /* Position derivative. */ 191 | temp.set_field(get_field()); 192 | 193 | /* Velocity derivative. */ 194 | temp.set_field(acceleration); 195 | 196 | return temp; 197 | } 198 | ``` 199 | 200 | ### Measurement model 201 | 202 | The measurement model is specified per field, in order to allow the expected 203 | measurement vector to be constructed for the dynamic measurement vector where 204 | not all measurements may be available each iteration. Each measurement model 205 | function takes a state vector as an input. 206 | 207 | The state vector type is provided to the measurement model specialisation as 208 | a template parameter; this allows a measurement vector class to be shared 209 | across multiple state vectors, with difference process model defined for 210 | each. 211 | 212 | Here is an example of a measurement model for a simple measurement vector and 213 | state vector: 214 | 215 | ```C++ 216 | using MyMeasurementVector = UKF::DynamicMeasurementVector< 217 | UKF::Field, 218 | UKF::Field, 219 | UKF::Field>, 220 | UKF::Field> 221 | >; 222 | 223 | using MyStateVector = UKF::StateVector< 224 | UKF::Field>, 225 | UKF::Field>, 226 | UKF::Field, 227 | UKF::Field 228 | >; 229 | 230 | template <> template <> 231 | UKF::Vector<3> MyMeasurementVector::expected_measurement 232 | (const MyStateVector& state) { 233 | return state.get_field() * UKF::Vector<3>(0, 0, -9.8); 234 | } 235 | 236 | template <> template <> 237 | UKF::Vector<3> MyMeasurementVector::expected_measurement 238 | (const MyStateVector& state) { 239 | return state.get_field(); 240 | } 241 | 242 | template <> template <> 243 | real_t MyMeasurementVector::expected_measurement 244 | (const MyStateVector& state) { 245 | return 101.3 - 1.2*(state.get_field() / 100.0); 246 | } 247 | 248 | template <> template <> 249 | real_t MyMeasurementVector::expected_measurement 250 | (const MyStateVector& state) { 251 | return 0.5 * 1.225 * state.get_field().squaredNorm(); 252 | } 253 | ``` 254 | 255 | As with the process model, the measurement model can take an arbitrary number 256 | of user-specified inputs: 257 | 258 | ```C++ 259 | template <> template <> 260 | UKF::Vector<3> MyMeasurementVector::expected_measurement 261 | >(const MyStateVector& state, const UKF::Vector<3>& input) { 262 | return state.get_field() * UKF::Vector<3>(0, 0, -9.8) + input; 263 | } 264 | 265 | template <> template <> 266 | UKF::Vector<3> MyMeasurementVector::expected_measurement 267 | >(const MyStateVector& state, const UKF::Vector<3>& input) { 268 | return state.get_field(); 269 | } 270 | 271 | template <> template <> 272 | real_t MyMeasurementVector::expected_measurement 273 | >(const MyStateVector& state, const UKF::Vector<3>& input) { 274 | return 101.3 - 1.2*(state.get_field() / 100.0); 275 | } 276 | 277 | template <> template <> 278 | real_t MyMeasurementVector::expected_measurement 279 | >(const MyStateVector& state, const UKF::Vector<3>& input) { 280 | return 0.5 * 1.225 * state.get_field().squaredNorm(); 281 | } 282 | ``` 283 | 284 | ### Initialisation 285 | 286 | The filter state, covariance, process noise covariance and measurement noise 287 | covariance should be initialised to appropriate values, e.g.: 288 | 289 | ```C++ 290 | MyCore test_filter; 291 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 292 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 293 | test_filter.state.set_field(UKF::Quaternion(1, 0, 0, 0)); 294 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 295 | test_filter.covariance = MyStateVector::CovarianceMatrix::Zero(); 296 | test_filter.covariance.diagonal() << 10000, 10000, 10000, 100, 100, 100, 1, 1, 5, 10, 10, 10; 297 | test_filter.process_noise_covariance = MyStateVector::CovarianceMatrix::Identity()*1e-5; 298 | test_filter.measurement_covariance << 10, 10, 10, 1, 1, 1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 0.05, 0.05, 0.05; 299 | 300 | ``` 301 | 302 | Or, for the SR-UKF: 303 | ```C++ 304 | MyCore test_filter; 305 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 306 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 307 | test_filter.state.set_field(UKF::Quaternion(1, 0, 0, 0)); 308 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 309 | test_filter.root_covariance = MyStateVector::CovarianceMatrix::Zero(); 310 | test_filter.root_covariance.diagonal() << 100, 100, 100, 10, 10, 10, 1, 1, 2.2, 3.2, 3.2, 3.2; 311 | test_filter.process_noise_root_covariance = MyStateVector::CovarianceMatrix::Identity()*3e-2; 312 | test_filter.measurement_root_covariance << 10, 10, 10, 1, 1, 1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 0.05, 0.05, 0.05; 313 | test_filter.measurement_root_covariance = test_filter.measurement_root_covariance.array().sqrt(); 314 | ``` 315 | 316 | Currently, only a diagonal measurement noise covariance matrix is supported. 317 | 318 | ### Iteration 319 | 320 | The general steps for carrying out a filter iteration are something like: 321 | 322 | ```C++ 323 | MyMeasurementVector meas; 324 | meas.set_field(UKF::Vector<3>(0.0, 0.0, 9.8)); 325 | meas.set_field(UKF::Vector<3>(0.0, 0.0, 0.0)); 326 | meas.set_field(101300.0); 327 | meas.set_field(101300.0); 328 | 329 | test_filter.step(0.01, meas); 330 | ``` 331 | 332 | Or, if it's necessary to do things with the internal filter state (e.g. 333 | filter health monitoring), then iteration can be split up into three steps: 334 | 335 | ```C++ 336 | MyMeasurementVector meas; 337 | meas.set_field(UKF::Vector<3>(0.0, 0.0, 9.8)); 338 | meas.set_field(UKF::Vector<3>(0.0, 0.0, 0.0)); 339 | meas.set_field(101300.0); 340 | meas.set_field(101300.0); 341 | 342 | test_filter.a_priori_step(0.01); 343 | /* 344 | At this point, the state and covariance (or root_covariance) variables 345 | reflect the a priori state and (root_)covariance. 346 | */ 347 | test_filter.innovation_step(meas); 348 | /* Innovation and innovation_(root_)covariance variables are now set. */ 349 | test_filter.a_posteriori_step(); 350 | /* State and (root_)covariance variables are set to the a priori values. 351 | ``` 352 | 353 | ## Building test and benchmark suites 354 | 355 | Build requires a compiler supporting C++17. 356 | 357 | First, clone the repo: 358 | 359 | git clone git@github.com:sfwa/ukf.git && cd ukf 360 | 361 | Then, configure CMake: 362 | 363 | cmake -B ./build -DCMAKE_BUILD_TYPE=Release 364 | 365 | Then build: 366 | 367 | cmake --build ./build --config Release 368 | 369 | Build and run the unit test suite: 370 | 371 | cd build && make unittest && test/unittest && cd .. 372 | 373 | If desired, build and run the benchmark suite: 374 | 375 | cd build && make benchmark && benchmark/benchmark && cd .. 376 | 377 | ## Examples 378 | 379 | Some examples are provided [here](examples/). 380 | 381 | ## References 382 | 383 | [1] "A New Extension of the Kalman Filter to Nonlinear Systems:, 384 | S. J. Julier and J. K. Uhlmann, 385 | https://www.cs.unc.edu/~welch/kalman/media/pdf/Julier1997_SPIE_KF.pdf 386 | 387 | [2] "Unscented Filtering for Spacecraft Attitude Estimation", John L. 388 | Crassidis and F. Landis Markley, http://www.acsu.buffalo.edu/~johnc/uf_att.pdf 389 | 390 | [3] "The Square-Root Unscented Kalman Filter for State and Parameter-Estimation", 391 | Rudolph van der Merwe and Eric A. Wan, 392 | http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.80.1421&rep=rep1&type=pdf 393 | -------------------------------------------------------------------------------- /benchmark/BenchmarkMain.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | BENCHMARK_MAIN(); 4 | -------------------------------------------------------------------------------- /benchmark/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8.4 FATAL_ERROR) 2 | PROJECT(benchmark) 3 | INCLUDE(ExternalProject) 4 | 5 | # Set default ExternalProject root directory 6 | SET_DIRECTORY_PROPERTIES(PROPERTIES EP_PREFIX .) 7 | 8 | # Add benchmark 9 | ExternalProject_Add( 10 | googlebenchmark 11 | URL https://github.com/google/benchmark/archive/v1.3.0.zip 12 | TIMEOUT 30 13 | CMAKE_ARGS -DCMAKE_BUILD_TYPE:STRING=Release 14 | # Disable install step 15 | INSTALL_COMMAND "" 16 | # Wrap download, configure and build steps in a script to log output 17 | LOG_DOWNLOAD ON 18 | LOG_CONFIGURE ON 19 | LOG_BUILD ON) 20 | 21 | # Disable dynamic memory allocation in Eigen 22 | ADD_DEFINITIONS(-DEIGEN_NO_MALLOC -DUKF_DOUBLE_PRECISION) 23 | 24 | # Specify include dir 25 | ExternalProject_Get_Property(googlebenchmark source_dir) 26 | SET(googlebenchmark_dir ${source_dir}) 27 | 28 | INCLUDE_DIRECTORIES(${googlebenchmark_dir}/include ../include ${eigen_dir}) 29 | 30 | # Add benchmark executable target 31 | ADD_EXECUTABLE(benchmark 32 | StateVectorBenchmark.cpp 33 | MeasurementVectorBenchmark.cpp 34 | CoreBenchmark.cpp 35 | SquareRootCoreBenchmark.cpp 36 | BenchmarkMain.cpp) 37 | 38 | # Create dependency of benchmark on googlebenchmark 39 | ADD_DEPENDENCIES(benchmark googlebenchmark eigen3) 40 | 41 | # Specify benchmark's link libraries 42 | ExternalProject_Get_Property(googlebenchmark binary_dir) 43 | TARGET_LINK_LIBRARIES(benchmark 44 | ${binary_dir}/src/${CMAKE_FIND_LIBRARY_PREFIXES}benchmark.a 45 | pthread) 46 | -------------------------------------------------------------------------------- /benchmark/CoreBenchmark.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "UKF/Types.h" 5 | #include "UKF/Integrator.h" 6 | #include "UKF/StateVector.h" 7 | #include "UKF/MeasurementVector.h" 8 | #include "UKF/Core.h" 9 | 10 | enum MyStateFields { 11 | Position, 12 | Velocity, 13 | Attitude, 14 | AngularVelocity 15 | }; 16 | 17 | using MyStateVector = UKF::StateVector< 18 | UKF::Field>, 19 | UKF::Field>, 20 | UKF::Field, 21 | UKF::Field> 22 | >; 23 | 24 | namespace UKF { 25 | namespace Parameters { 26 | template <> constexpr real_t AlphaSquared = 1e-6; 27 | } 28 | 29 | /* 30 | State vector process model. One version takes body frame kinematic 31 | acceleration and angular acceleration as inputs, the other doesn't (assumes 32 | zero accelerations). 33 | */ 34 | template <> template <> 35 | MyStateVector MyStateVector::derivative, UKF::Vector<3>>( 36 | const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) const { 37 | MyStateVector temp; 38 | 39 | /* Position derivative. */ 40 | temp.set_field(get_field()); 41 | 42 | /* Velocity derivative. */ 43 | temp.set_field(get_field().conjugate() * acceleration); 44 | 45 | /* Attitude derivative. */ 46 | UKF::Quaternion temp_q; 47 | temp_q.vec() = get_field(); 48 | temp_q.w() = 0; 49 | temp.set_field(temp_q); 50 | 51 | /* Angular velocity derivative. */ 52 | temp.set_field(angular_acceleration); 53 | 54 | return temp; 55 | } 56 | 57 | template <> template <> 58 | MyStateVector MyStateVector::derivative<>() const { 59 | return derivative(UKF::Vector<3>(0, 0, 0), UKF::Vector<3>(0, 0, 0)); 60 | } 61 | 62 | } 63 | 64 | /* Set up measurement vector class. */ 65 | enum MyMeasurementFields { 66 | GPS_Position, 67 | GPS_Velocity, 68 | Accelerometer, 69 | Magnetometer, 70 | Gyroscope 71 | }; 72 | 73 | using MyMeasurementVector = UKF::DynamicMeasurementVector< 74 | UKF::Field>, 75 | UKF::Field>, 76 | UKF::Field>, 77 | UKF::Field, 78 | UKF::Field> 79 | >; 80 | 81 | using MyCore = UKF::Core< 82 | MyStateVector, 83 | MyMeasurementVector, 84 | UKF::IntegratorRK4 85 | >; 86 | 87 | namespace UKF { 88 | /* 89 | Define measurement model to be used in tests. NOTE: These are just for 90 | testing, don't expect them to make any physical sense whatsoever. 91 | */ 92 | template <> template <> 93 | UKF::Vector<3> MyMeasurementVector::expected_measurement 94 | (const MyStateVector& state) { 95 | return state.get_field(); 96 | } 97 | 98 | template <> template <> 99 | UKF::Vector<3> MyMeasurementVector::expected_measurement 100 | (const MyStateVector& state) { 101 | return state.get_field(); 102 | } 103 | 104 | template <> template <> 105 | UKF::Vector<3> MyMeasurementVector::expected_measurement 106 | (const MyStateVector& state) { 107 | return state.get_field() * UKF::Vector<3>(0, 0, -9.8); 108 | } 109 | 110 | template <> template <> 111 | UKF::FieldVector MyMeasurementVector::expected_measurement 112 | (const MyStateVector& state) { 113 | return state.get_field() * UKF::FieldVector(1, 0, 0); 114 | } 115 | 116 | template <> template <> 117 | UKF::Vector<3> MyMeasurementVector::expected_measurement 118 | (const MyStateVector& state) { 119 | return state.get_field(); 120 | } 121 | 122 | } 123 | 124 | MyCore create_initialised_test_filter() { 125 | MyCore test_filter; 126 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 127 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 128 | test_filter.state.set_field(UKF::Quaternion(1, 0, 0, 0)); 129 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 130 | test_filter.covariance = MyStateVector::CovarianceMatrix::Zero(); 131 | test_filter.covariance.diagonal() << 10000, 10000, 10000, 100, 100, 100, 1, 1, 5, 10, 10, 10; 132 | test_filter.measurement_covariance << 10, 10, 10, 1, 1, 1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 0.05, 0.05, 0.05; 133 | 134 | real_t a, b; 135 | real_t dt = 0.01; 136 | a = std::sqrt(0.1*dt*dt); 137 | b = std::sqrt(0.1*dt); 138 | test_filter.process_noise_covariance << a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 139 | 0, a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 140 | 0, 0, a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 141 | 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 0, 0, 142 | 0, 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 0, 143 | 0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 144 | 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 0, 145 | 0, 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 146 | 0, 0, 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 147 | 0, 0, 0, 0, 0, 0, 0, 0, 0, b, 0, 0, 148 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, b, 0, 149 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, b; 150 | 151 | return test_filter; 152 | } 153 | 154 | void Core_APrioriStep(benchmark::State& state) { 155 | MyCore test_filter = create_initialised_test_filter(); 156 | 157 | while(state.KeepRunning()) { 158 | test_filter.a_priori_step(0.01); 159 | } 160 | } 161 | 162 | BENCHMARK(Core_APrioriStep); 163 | 164 | void Core_InnovationStep(benchmark::State& state) { 165 | MyCore test_filter = create_initialised_test_filter(); 166 | MyMeasurementVector m; 167 | 168 | m.set_field(UKF::Vector<3>(100, 10, -50)); 169 | m.set_field(UKF::Vector<3>(20, 0, 0)); 170 | m.set_field(UKF::Vector<3>(0, 0, -9.8)); 171 | m.set_field(UKF::FieldVector(0, -1, 0)); 172 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 173 | 174 | test_filter.a_priori_step(0.01); 175 | 176 | while(state.KeepRunning()) { 177 | test_filter.innovation_step(m); 178 | } 179 | } 180 | 181 | BENCHMARK(Core_InnovationStep); 182 | 183 | void Core_APosterioriStep(benchmark::State& state) { 184 | MyCore test_filter = create_initialised_test_filter(); 185 | MyMeasurementVector m; 186 | 187 | m.set_field(UKF::Vector<3>(100, 10, -50)); 188 | m.set_field(UKF::Vector<3>(20, 0, 0)); 189 | m.set_field(UKF::Vector<3>(0, 0, -9.8)); 190 | m.set_field(UKF::FieldVector(0, -1, 0)); 191 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 192 | 193 | test_filter.a_priori_step(0.01); 194 | test_filter.innovation_step(m); 195 | 196 | MyStateVector::CovarianceMatrix initial_cov = test_filter.covariance; 197 | MyStateVector initial_state = test_filter.state; 198 | 199 | while(state.KeepRunning()) { 200 | test_filter.covariance = initial_cov; 201 | test_filter.state = initial_state; 202 | test_filter.a_posteriori_step(); 203 | } 204 | } 205 | 206 | BENCHMARK(Core_APosterioriStep); 207 | -------------------------------------------------------------------------------- /benchmark/MeasurementVectorBenchmark.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "UKF/Types.h" 5 | #include "UKF/StateVector.h" 6 | #include "UKF/MeasurementVector.h" 7 | 8 | enum MyStateVectorFields { 9 | AngularVelocity, 10 | Altitude, 11 | Velocity, 12 | Attitude 13 | }; 14 | 15 | using MyStateVector = UKF::StateVector< 16 | UKF::Field>, 17 | UKF::Field>, 18 | UKF::Field, 19 | UKF::Field 20 | >; 21 | 22 | enum MyFields { 23 | StaticPressure, 24 | DynamicPressure, 25 | Accelerometer, 26 | Gyroscope 27 | }; 28 | 29 | using MV_Fixed = UKF::FixedMeasurementVector< 30 | UKF::Field>, 31 | UKF::Field>, 32 | UKF::Field, 33 | UKF::Field 34 | >; 35 | 36 | namespace UKF { 37 | template <> template <> 38 | UKF::Vector<3> MV_Fixed::expected_measurement 39 | (const MyStateVector& state) { 40 | return state.get_field() * UKF::Vector<3>(0, 0, -9.8); 41 | } 42 | 43 | template <> template <> 44 | UKF::Vector<3> MV_Fixed::expected_measurement 45 | (const MyStateVector& state) { 46 | return state.get_field(); 47 | } 48 | 49 | template <> template <> 50 | real_t MV_Fixed::expected_measurement 51 | (const MyStateVector& state) { 52 | return 101.3 - 1.2*(state.get_field() / 100.0); 53 | } 54 | 55 | template <> template <> 56 | real_t MV_Fixed::expected_measurement 57 | (const MyStateVector& state) { 58 | return 0.5 * 1.225 * state.get_field().squaredNorm(); 59 | } 60 | 61 | } 62 | 63 | using MV_Dynamic = UKF::DynamicMeasurementVector< 64 | UKF::Field>, 65 | UKF::Field>, 66 | UKF::Field, 67 | UKF::Field 68 | >; 69 | 70 | namespace UKF { 71 | template <> template <> 72 | UKF::Vector<3> MV_Dynamic::expected_measurement 73 | (const MyStateVector& state) { 74 | return state.get_field() * UKF::Vector<3>(0, 0, -9.8); 75 | } 76 | 77 | template <> template <> 78 | UKF::Vector<3> MV_Dynamic::expected_measurement 79 | (const MyStateVector& state) { 80 | return state.get_field(); 81 | } 82 | 83 | template <> template <> 84 | real_t MV_Dynamic::expected_measurement 85 | (const MyStateVector& state) { 86 | return 101.3 - 1.2*(state.get_field() / 100.0); 87 | } 88 | 89 | template <> template <> 90 | real_t MV_Dynamic::expected_measurement 91 | (const MyStateVector& state) { 92 | return 0.5 * 1.225 * state.get_field().squaredNorm(); 93 | } 94 | 95 | } 96 | 97 | /* 98 | Tests to compare set/get performance between fixed and dynamic measurement vectors. 99 | */ 100 | void MeasurementVectorFixed_SetGetField(benchmark::State& state) { 101 | MV_Fixed test_measurement; 102 | while(state.KeepRunning()) { 103 | test_measurement.set_field(UKF::Vector<3>(1, 2, 3)); 104 | benchmark::DoNotOptimize(test_measurement.get_field()); 105 | } 106 | } 107 | 108 | BENCHMARK(MeasurementVectorFixed_SetGetField); 109 | 110 | void MeasurementVectorDynamic_SetGetField(benchmark::State& state) { 111 | MV_Dynamic test_measurement; 112 | while(state.KeepRunning()) { 113 | test_measurement.set_field(UKF::Vector<3>(1, 2, 3)); 114 | benchmark::DoNotOptimize(test_measurement.get_field()); 115 | } 116 | } 117 | 118 | BENCHMARK(MeasurementVectorDynamic_SetGetField); 119 | 120 | void MeasurementVectorFixed_SigmaPointGeneration(benchmark::State& state) { 121 | MyStateVector test_state; 122 | MV_Fixed test_measurement; 123 | 124 | test_measurement.set_field(UKF::Vector<3>(0, 0, 0)); 125 | test_measurement.set_field(UKF::Vector<3>(0, 0, 0)); 126 | test_measurement.set_field(0); 127 | test_measurement.set_field(0); 128 | 129 | test_state.set_field(UKF::Vector<3>(1, 2, 3)); 130 | test_state.set_field(UKF::Vector<3>(1, 0, 0)); 131 | test_state.set_field(UKF::Quaternion(1, 0, 0, 0)); 132 | test_state.set_field(1000); 133 | 134 | MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero(); 135 | covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; 136 | 137 | MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution(covariance); 138 | 139 | while(state.KeepRunning()) { 140 | benchmark::DoNotOptimize(test_measurement.calculate_sigma_point_distribution(sigma_points)); 141 | } 142 | } 143 | 144 | BENCHMARK(MeasurementVectorFixed_SigmaPointGeneration); 145 | 146 | void MeasurementVectorDynamic_SigmaPointGeneration(benchmark::State& state) { 147 | MyStateVector test_state; 148 | MV_Dynamic test_measurement; 149 | 150 | test_measurement.set_field(UKF::Vector<3>(0, 0, 0)); 151 | test_measurement.set_field(UKF::Vector<3>(0, 0, 0)); 152 | test_measurement.set_field(0); 153 | test_measurement.set_field(0); 154 | 155 | test_state.set_field(UKF::Vector<3>(1, 2, 3)); 156 | test_state.set_field(UKF::Vector<3>(1, 0, 0)); 157 | test_state.set_field(UKF::Quaternion(1, 0, 0, 0)); 158 | test_state.set_field(1000); 159 | 160 | MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero(); 161 | covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; 162 | 163 | MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution(covariance); 164 | 165 | while(state.KeepRunning()) { 166 | benchmark::DoNotOptimize(test_measurement.calculate_sigma_point_distribution(sigma_points)); 167 | } 168 | } 169 | 170 | BENCHMARK(MeasurementVectorDynamic_SigmaPointGeneration); 171 | 172 | void MeasurementVectorFixed_FullMeasurementCalculation(benchmark::State& state) { 173 | MyStateVector test_state; 174 | MV_Fixed test_measurement; 175 | 176 | test_measurement.set_field(UKF::Vector<3>(0, 0, 0)); 177 | test_measurement.set_field(UKF::Vector<3>(0, 0, 0)); 178 | test_measurement.set_field(0); 179 | test_measurement.set_field(0); 180 | 181 | test_state.set_field(UKF::Vector<3>(1, 2, 3)); 182 | test_state.set_field(UKF::Vector<3>(1, 0, 0)); 183 | test_state.set_field(UKF::Quaternion(1, 0, 0, 0)); 184 | test_state.set_field(1000); 185 | 186 | MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero(); 187 | covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; 188 | 189 | MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution(covariance); 190 | 191 | MV_Fixed::SigmaPointDistribution measurement_sigma_points; 192 | MV_Fixed mean_measurement; 193 | MV_Fixed::SigmaPointDeltas sigma_point_deltas; 194 | MV_Fixed::CovarianceMatrix calculated_covariance; 195 | 196 | while(state.KeepRunning()) { 197 | measurement_sigma_points = test_measurement.calculate_sigma_point_distribution(sigma_points); 198 | mean_measurement = test_measurement.calculate_sigma_point_mean(measurement_sigma_points); 199 | sigma_point_deltas = mean_measurement.calculate_sigma_point_deltas(measurement_sigma_points); 200 | calculated_covariance = mean_measurement.calculate_sigma_point_covariance(sigma_point_deltas); 201 | } 202 | } 203 | 204 | BENCHMARK(MeasurementVectorFixed_FullMeasurementCalculation); 205 | 206 | void MeasurementVectorDynamic_FullMeasurementCalculation(benchmark::State& state) { 207 | MyStateVector test_state; 208 | MV_Dynamic test_measurement; 209 | 210 | test_measurement.set_field(UKF::Vector<3>(0, 0, 0)); 211 | test_measurement.set_field(UKF::Vector<3>(0, 0, 0)); 212 | test_measurement.set_field(0); 213 | test_measurement.set_field(0); 214 | 215 | test_state.set_field(UKF::Vector<3>(1, 2, 3)); 216 | test_state.set_field(UKF::Vector<3>(1, 0, 0)); 217 | test_state.set_field(UKF::Quaternion(1, 0, 0, 0)); 218 | test_state.set_field(1000); 219 | 220 | MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero(); 221 | covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; 222 | 223 | MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution(covariance); 224 | 225 | MV_Dynamic::SigmaPointDistribution measurement_sigma_points( 226 | test_measurement.size(), MyStateVector::num_sigma()); 227 | MV_Dynamic mean_measurement(test_measurement.size()); 228 | MV_Dynamic::SigmaPointDeltas sigma_point_deltas(test_measurement.size(), MyStateVector::num_sigma()); 229 | MV_Dynamic::CovarianceMatrix calculated_covariance(test_measurement.size(), test_measurement.size()); 230 | 231 | while(state.KeepRunning()) { 232 | measurement_sigma_points = test_measurement.calculate_sigma_point_distribution(sigma_points); 233 | mean_measurement = test_measurement.calculate_sigma_point_mean(measurement_sigma_points); 234 | sigma_point_deltas = mean_measurement.calculate_sigma_point_deltas(measurement_sigma_points); 235 | calculated_covariance = mean_measurement.calculate_sigma_point_covariance(sigma_point_deltas); 236 | } 237 | } 238 | 239 | BENCHMARK(MeasurementVectorDynamic_FullMeasurementCalculation); 240 | 241 | void MeasurementVectorFixed_MeasurementCovariance(benchmark::State& state) { 242 | MV_Fixed test_measurement, expected_measurement; 243 | MV_Fixed::CovarianceVector measurement_covariance; 244 | 245 | measurement_covariance.set_field(UKF::Vector<3>(1, 2, 3)); 246 | measurement_covariance.set_field(UKF::Vector<3>(4, 5, 6)); 247 | measurement_covariance.set_field(7); 248 | measurement_covariance.set_field(8); 249 | 250 | expected_measurement.set_field(UKF::Vector<3>(1, 2, 3)); 251 | expected_measurement.set_field(UKF::Vector<3>(4, 5, 6)); 252 | expected_measurement.set_field(7); 253 | expected_measurement.set_field(8); 254 | 255 | while(state.KeepRunning()) { 256 | benchmark::DoNotOptimize(test_measurement.calculate_measurement_covariance( 257 | measurement_covariance, expected_measurement)); 258 | } 259 | } 260 | 261 | BENCHMARK(MeasurementVectorFixed_MeasurementCovariance); 262 | 263 | void MeasurementVectorDynamic_MeasurementCovariance(benchmark::State& state) { 264 | MV_Dynamic test_measurement, expected_measurement; 265 | MV_Dynamic::CovarianceVector measurement_covariance; 266 | 267 | measurement_covariance.set_field(UKF::Vector<3>(1, 2, 3)); 268 | measurement_covariance.set_field(UKF::Vector<3>(4, 5, 6)); 269 | measurement_covariance.set_field(7); 270 | measurement_covariance.set_field(8); 271 | 272 | expected_measurement.set_field(UKF::Vector<3>(1, 2, 3)); 273 | expected_measurement.set_field(UKF::Vector<3>(4, 5, 6)); 274 | expected_measurement.set_field(7); 275 | expected_measurement.set_field(8); 276 | 277 | while(state.KeepRunning()) { 278 | benchmark::DoNotOptimize(test_measurement.calculate_measurement_covariance( 279 | measurement_covariance, expected_measurement)); 280 | } 281 | } 282 | 283 | BENCHMARK(MeasurementVectorDynamic_MeasurementCovariance); 284 | -------------------------------------------------------------------------------- /benchmark/SquareRootCoreBenchmark.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "UKF/Types.h" 5 | #include "UKF/Integrator.h" 6 | #include "UKF/StateVector.h" 7 | #include "UKF/MeasurementVector.h" 8 | #include "UKF/Core.h" 9 | 10 | /* 11 | Set up state vector class. The order of these is changed to prevent 12 | linker collisions with the ones in CoreBenchmark.cpp. 13 | */ 14 | enum MyStateFields { 15 | Attitude, 16 | AngularVelocity, 17 | Position, 18 | Velocity 19 | }; 20 | 21 | using MyStateVector = UKF::StateVector< 22 | UKF::Field>, 23 | UKF::Field>, 24 | UKF::Field, 25 | UKF::Field> 26 | >; 27 | 28 | namespace UKF { 29 | namespace Parameters { 30 | template <> constexpr real_t AlphaSquared = 1e-6; 31 | } 32 | 33 | 34 | /* 35 | State vector process model. One version takes body frame kinematic 36 | acceleration and angular acceleration as inputs, the other doesn't (assumes 37 | zero accelerations). 38 | */ 39 | template <> template <> 40 | MyStateVector MyStateVector::derivative, UKF::Vector<3>>( 41 | const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) const { 42 | MyStateVector temp; 43 | 44 | /* Position derivative. */ 45 | temp.set_field(get_field()); 46 | 47 | /* Velocity derivative. */ 48 | temp.set_field(get_field().conjugate() * acceleration); 49 | 50 | /* Attitude derivative. */ 51 | UKF::Quaternion temp_q; 52 | temp_q.vec() = get_field(); 53 | temp_q.w() = 0; 54 | temp.set_field(temp_q); 55 | 56 | /* Angular velocity derivative. */ 57 | temp.set_field(angular_acceleration); 58 | 59 | return temp; 60 | } 61 | 62 | template <> template <> 63 | MyStateVector MyStateVector::derivative<>() const { 64 | return derivative(UKF::Vector<3>(0, 0, 0), UKF::Vector<3>(0, 0, 0)); 65 | } 66 | 67 | } 68 | 69 | /* 70 | Set up measurement vector class. The order of these is changed to prevent 71 | linker collisions with the ones in CoreBenchmark.cpp. 72 | */ 73 | enum MyMeasurementFields { 74 | Accelerometer, 75 | Magnetometer, 76 | Gyroscope, 77 | GPS_Position, 78 | GPS_Velocity 79 | }; 80 | 81 | using MyMeasurementVector = UKF::DynamicMeasurementVector< 82 | UKF::Field>, 83 | UKF::Field>, 84 | UKF::Field>, 85 | UKF::Field, 86 | UKF::Field> 87 | >; 88 | 89 | using MyCore = UKF::SquareRootCore< 90 | MyStateVector, 91 | MyMeasurementVector, 92 | UKF::IntegratorRK4 93 | >; 94 | 95 | namespace UKF { 96 | /* 97 | Define measurement model to be used in tests. NOTE: These are just for 98 | testing, don't expect them to make any physical sense whatsoever. 99 | */ 100 | template <> template <> 101 | UKF::Vector<3> MyMeasurementVector::expected_measurement 102 | (const MyStateVector& state) { 103 | return state.get_field(); 104 | } 105 | 106 | template <> template <> 107 | UKF::Vector<3> MyMeasurementVector::expected_measurement 108 | (const MyStateVector& state) { 109 | return state.get_field(); 110 | } 111 | 112 | template <> template <> 113 | UKF::Vector<3> MyMeasurementVector::expected_measurement 114 | (const MyStateVector& state) { 115 | return state.get_field() * UKF::Vector<3>(0, 0, -9.8); 116 | } 117 | 118 | template <> template <> 119 | UKF::FieldVector MyMeasurementVector::expected_measurement 120 | (const MyStateVector& state) { 121 | return state.get_field() * UKF::FieldVector(1, 0, 0); 122 | } 123 | 124 | template <> template <> 125 | UKF::Vector<3> MyMeasurementVector::expected_measurement 126 | (const MyStateVector& state) { 127 | return state.get_field(); 128 | } 129 | 130 | } 131 | 132 | /* 133 | Initialise covariances as square roots to be comparable with CoreBenchmark.cpp. 134 | */ 135 | MyCore create_initialised_sr_test_filter() { 136 | MyCore test_filter; 137 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 138 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 139 | test_filter.state.set_field(UKF::Quaternion(1, 0, 0, 0)); 140 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 141 | test_filter.root_covariance = MyStateVector::CovarianceMatrix::Zero(); 142 | test_filter.root_covariance.diagonal() << 143 | 10000, 10000, 10000, 100, 100, 100, 1, 1, 5, 10, 10, 10; 144 | test_filter.root_covariance = test_filter.root_covariance.llt().matrixU(); 145 | test_filter.measurement_root_covariance << 146 | 10, 10, 10, 1, 1, 1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 0.05, 0.05, 0.05; 147 | test_filter.measurement_root_covariance = test_filter.measurement_root_covariance.array().sqrt(); 148 | 149 | real_t a, b; 150 | real_t dt = 0.01; 151 | a = std::sqrt(0.1*dt*dt); 152 | b = std::sqrt(0.1*dt); 153 | test_filter.process_noise_root_covariance << a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 154 | 0, a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155 | 0, 0, a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 156 | 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 0, 0, 157 | 0, 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 0, 158 | 0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 159 | 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 0, 160 | 0, 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 161 | 0, 0, 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 162 | 0, 0, 0, 0, 0, 0, 0, 0, 0, b, 0, 0, 163 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, b, 0, 164 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, b; 165 | test_filter.process_noise_root_covariance = test_filter.process_noise_root_covariance.llt().matrixU(); 166 | 167 | return test_filter; 168 | } 169 | 170 | void SquareRootCore_APrioriStep(benchmark::State& state) { 171 | MyCore test_filter = create_initialised_sr_test_filter(); 172 | 173 | while(state.KeepRunning()) { 174 | test_filter.a_priori_step(0.01); 175 | } 176 | } 177 | 178 | BENCHMARK(SquareRootCore_APrioriStep); 179 | 180 | void SquareRootCore_InnovationStep(benchmark::State& state) { 181 | MyCore test_filter = create_initialised_sr_test_filter(); 182 | MyMeasurementVector m; 183 | 184 | m.set_field(UKF::Vector<3>(100, 10, -50)); 185 | m.set_field(UKF::Vector<3>(20, 0, 0)); 186 | m.set_field(UKF::Vector<3>(0, 0, -9.8)); 187 | m.set_field(UKF::FieldVector(0, -1, 0)); 188 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 189 | 190 | test_filter.a_priori_step(0.01); 191 | 192 | while(state.KeepRunning()) { 193 | test_filter.innovation_step(m); 194 | } 195 | } 196 | 197 | BENCHMARK(SquareRootCore_InnovationStep); 198 | 199 | void SquareRootCore_APosterioriStep(benchmark::State& state) { 200 | MyCore test_filter = create_initialised_sr_test_filter(); 201 | MyMeasurementVector m; 202 | 203 | m.set_field(UKF::Vector<3>(100, 10, -50)); 204 | m.set_field(UKF::Vector<3>(20, 0, 0)); 205 | m.set_field(UKF::Vector<3>(0, 0, -9.8)); 206 | m.set_field(UKF::FieldVector(0, -1, 0)); 207 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 208 | 209 | test_filter.a_priori_step(0.01); 210 | test_filter.innovation_step(m); 211 | 212 | MyStateVector::CovarianceMatrix initial_cov = test_filter.root_covariance; 213 | MyStateVector initial_state = test_filter.state; 214 | 215 | while(state.KeepRunning()) { 216 | test_filter.root_covariance = initial_cov; 217 | test_filter.state = initial_state; 218 | test_filter.a_posteriori_step(); 219 | } 220 | } 221 | 222 | BENCHMARK(SquareRootCore_APosterioriStep); 223 | -------------------------------------------------------------------------------- /benchmark/StateVectorBenchmark.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "UKF/Types.h" 5 | #include "UKF/Integrator.h" 6 | #include "UKF/StateVector.h" 7 | 8 | using SV16_FourQuaternions = UKF::StateVector< 9 | UKF::Field<1, UKF::Quaternion>, 10 | UKF::Field<2, UKF::Quaternion>, 11 | UKF::Field<3, UKF::Quaternion>, 12 | UKF::Field<4, UKF::Quaternion> 13 | >; 14 | 15 | using SV16_FourVectors = UKF::StateVector< 16 | UKF::Field<1, UKF::Vector<4>>, 17 | UKF::Field<2, UKF::Vector<4>>, 18 | UKF::Field<3, UKF::Vector<4>>, 19 | UKF::Field<4, UKF::Vector<4>> 20 | >; 21 | 22 | using SV16_OneVector = UKF::StateVector< 23 | UKF::Field<1, UKF::Vector<16>> 24 | >; 25 | 26 | /* 27 | Tests to ensure setField() and getField() don't hurt performance compared to 28 | segment(). 29 | */ 30 | void StateVector_SetGetUsingSegment(benchmark::State& state) { 31 | SV16_FourVectors test_state; 32 | test_state.segment<4>(0) << UKF::Vector<4>(1, 2, 3, 4); 33 | while(state.KeepRunning()) { 34 | test_state.segment<4>(0) << UKF::Vector<4>(1, 2, 3, 4); 35 | benchmark::DoNotOptimize(test_state.segment<4>(0)); 36 | } 37 | } 38 | 39 | void StateVector_SetGetUsingSetField(benchmark::State& state) { 40 | SV16_FourVectors test_state; 41 | test_state.segment<4>(0) << UKF::Vector<4>(1, 2, 3, 4); 42 | while(state.KeepRunning()) { 43 | test_state.set_field<1>(UKF::Vector<4>(1, 2, 3, 4)); 44 | benchmark::DoNotOptimize(test_state.get_field<1>()); 45 | } 46 | } 47 | 48 | BENCHMARK(StateVector_SetGetUsingSegment); 49 | BENCHMARK(StateVector_SetGetUsingSetField); 50 | 51 | template 52 | void StateVector_SigmaPointGeneration(benchmark::State& state) { 53 | T test_state; 54 | test_state << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1; 55 | typename T::CovarianceMatrix covariance = T::CovarianceMatrix::Zero(); 56 | covariance.diagonal() << UKF::Vector::Ones(); 57 | while(state.KeepRunning()) { 58 | benchmark::DoNotOptimize(test_state.calculate_sigma_point_distribution(covariance)); 59 | } 60 | } 61 | 62 | BENCHMARK_TEMPLATE(StateVector_SigmaPointGeneration, SV16_OneVector); 63 | BENCHMARK_TEMPLATE(StateVector_SigmaPointGeneration, SV16_FourVectors); 64 | BENCHMARK_TEMPLATE(StateVector_SigmaPointGeneration, SV16_FourQuaternions); 65 | 66 | template 67 | void StateVector_SigmaPointMean(benchmark::State& state) { 68 | T test_state; 69 | test_state << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1; 70 | typename T::CovarianceMatrix covariance = T::CovarianceMatrix::Zero(); 71 | covariance.diagonal() << UKF::Vector::Ones(); 72 | typename T::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution(covariance); 73 | while(state.KeepRunning()) { 74 | benchmark::DoNotOptimize(T::calculate_sigma_point_mean(sigma_points)); 75 | } 76 | } 77 | 78 | BENCHMARK_TEMPLATE(StateVector_SigmaPointMean, SV16_OneVector); 79 | BENCHMARK_TEMPLATE(StateVector_SigmaPointMean, SV16_FourVectors); 80 | BENCHMARK_TEMPLATE(StateVector_SigmaPointMean, SV16_FourQuaternions); 81 | 82 | template 83 | void StateVector_SigmaPointDeltas(benchmark::State& state) { 84 | T test_state; 85 | test_state << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1; 86 | typename T::CovarianceMatrix covariance = T::CovarianceMatrix::Zero(); 87 | covariance.diagonal() << UKF::Vector::Ones(); 88 | typename T::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution(covariance); 89 | T test_mean = T::calculate_sigma_point_mean(sigma_points); 90 | while(state.KeepRunning()) { 91 | benchmark::DoNotOptimize(test_mean.calculate_sigma_point_deltas(sigma_points)); 92 | } 93 | } 94 | 95 | BENCHMARK_TEMPLATE(StateVector_SigmaPointDeltas, SV16_OneVector); 96 | BENCHMARK_TEMPLATE(StateVector_SigmaPointDeltas, SV16_FourVectors); 97 | BENCHMARK_TEMPLATE(StateVector_SigmaPointDeltas, SV16_FourQuaternions); 98 | 99 | template 100 | void StateVector_SigmaPointCovariance(benchmark::State& state) { 101 | T test_state; 102 | test_state << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1; 103 | typename T::CovarianceMatrix covariance = T::CovarianceMatrix::Zero(); 104 | covariance.diagonal() << UKF::Vector::Ones(); 105 | typename T::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution(covariance); 106 | T test_mean = T::calculate_sigma_point_mean(sigma_points); 107 | typename T::SigmaPointDeltas sigma_point_deltas = test_mean.calculate_sigma_point_deltas(sigma_points); 108 | while(state.KeepRunning()) { 109 | benchmark::DoNotOptimize(T::calculate_sigma_point_covariance(sigma_point_deltas)); 110 | } 111 | } 112 | 113 | BENCHMARK_TEMPLATE(StateVector_SigmaPointCovariance, SV16_OneVector); 114 | BENCHMARK_TEMPLATE(StateVector_SigmaPointCovariance, SV16_FourVectors); 115 | BENCHMARK_TEMPLATE(StateVector_SigmaPointCovariance, SV16_FourQuaternions); 116 | 117 | template 118 | void StateVector_FullAPrioriCalculation(benchmark::State& state) { 119 | T test_state; 120 | test_state << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1; 121 | typename T::CovarianceMatrix covariance = T::CovarianceMatrix::Zero(); 122 | covariance.diagonal() << UKF::Vector::Ones(); 123 | typename T::SigmaPointDistribution sigma_points; 124 | typename T::SigmaPointDeltas sigma_point_deltas; 125 | T test_mean; 126 | typename T::CovarianceMatrix apriori_covariance; 127 | while(state.KeepRunning()) { 128 | sigma_points = test_state.calculate_sigma_point_distribution(covariance); 129 | test_mean = T::calculate_sigma_point_mean(sigma_points); 130 | sigma_point_deltas = test_mean.calculate_sigma_point_deltas(sigma_points); 131 | apriori_covariance = T::calculate_sigma_point_covariance(sigma_point_deltas); 132 | } 133 | } 134 | 135 | BENCHMARK_TEMPLATE(StateVector_FullAPrioriCalculation, SV16_OneVector); 136 | BENCHMARK_TEMPLATE(StateVector_FullAPrioriCalculation, SV16_FourVectors); 137 | BENCHMARK_TEMPLATE(StateVector_FullAPrioriCalculation, SV16_FourQuaternions); 138 | 139 | template 140 | void StateVector_UpdateDelta(benchmark::State& state) { 141 | T test_state; 142 | test_state << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1; 143 | typename T::StateVectorDelta test_delta = T::StateVectorDelta::Ones(); 144 | while(state.KeepRunning()) { 145 | test_state.apply_delta(test_delta); 146 | } 147 | } 148 | 149 | BENCHMARK_TEMPLATE(StateVector_UpdateDelta, SV16_OneVector); 150 | BENCHMARK_TEMPLATE(StateVector_UpdateDelta, SV16_FourVectors); 151 | BENCHMARK_TEMPLATE(StateVector_UpdateDelta, SV16_FourQuaternions); 152 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8.4 FATAL_ERROR) 2 | PROJECT(examples) 3 | 4 | MACRO(SUBDIRLIST result curdir) 5 | FILE(GLOB children RELATIVE ${curdir} ${curdir}/*) 6 | SET(dirlist "") 7 | FOREACH(child ${children}) 8 | IF(IS_DIRECTORY ${curdir}/${child}) 9 | LIST(APPEND dirlist ${child}) 10 | ENDIF() 11 | ENDFOREACH() 12 | SET(${result} ${dirlist}) 13 | ENDMACRO() 14 | 15 | SUBDIRLIST(SUBDIRS ${CMAKE_CURRENT_SOURCE_DIR}) 16 | 17 | FOREACH(subdir ${SUBDIRS}) 18 | ADD_SUBDIRECTORY(${subdir}) 19 | ENDFOREACH() 20 | -------------------------------------------------------------------------------- /examples/ahrs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8.4 FATAL_ERROR) 2 | PROJECT(ahrs) 3 | 4 | # Disable dynamic memory allocation in Eigen 5 | ADD_DEFINITIONS(-DEIGEN_NO_MALLOC -DUKF_SINGLE_PRECISION) 6 | 7 | INCLUDE_DIRECTORIES(../../include ${eigen_dir}) 8 | 9 | cmake_policy(SET CMP0042 NEW) 10 | 11 | ADD_LIBRARY(ahrs SHARED ahrs.cpp) 12 | SET_TARGET_PROPERTIES(ahrs PROPERTIES OUTPUT_NAME ahrs) -------------------------------------------------------------------------------- /examples/ahrs/ahrs.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "UKF/Types.h" 5 | #include "UKF/Integrator.h" 6 | #include "UKF/StateVector.h" 7 | #include "UKF/MeasurementVector.h" 8 | #include "UKF/Core.h" 9 | #include "ahrs.h" 10 | 11 | /* 12 | This is an implementation of an Unscented Kalman filter for a 9-axis AHRS, 13 | using accelerometer, gyroscope and magnetometer to estimate attitude and 14 | angular velocity. 15 | */ 16 | 17 | /* Value of g in m/s^2. */ 18 | #define G_ACCEL (9.80665) 19 | 20 | /* Default magnetic field norm in Gauss. */ 21 | #define MAG_NORM (0.45) 22 | 23 | enum AHRS_Keys { 24 | /* AHRS filter fields. */ 25 | Attitude, 26 | AngularVelocity, 27 | 28 | /* Parameter estimation filter fields. */ 29 | AccelerometerBias, 30 | GyroscopeBias, 31 | MagnetometerBias, 32 | MagnetometerScaleFactor, 33 | MagneticFieldNorm, 34 | MagneticFieldInclination, 35 | 36 | /* AHRS measurement vector fields. */ 37 | Accelerometer, 38 | Gyroscope, 39 | Magnetometer 40 | }; 41 | 42 | /* 43 | The AHRS state vector contains the following: 44 | - Attitude as a quaternion (NED frame to body frame) 45 | - Angular velocity (body frame, rad/s) 46 | */ 47 | using AHRS_StateVector = UKF::StateVector< 48 | UKF::Field, 49 | UKF::Field> 50 | >; 51 | 52 | namespace UKF { 53 | namespace Parameters { 54 | template <> constexpr real_t AlphaSquared = 1e-2; 55 | template <> constexpr real_t Kappa = 3.0; 56 | } 57 | } 58 | 59 | /* 60 | In addition to the AHRS filter, an online parameter estimation filter is also 61 | implemented in order to calculate biases in each of the sensors. 62 | The magnetometer scale factor is represented as a direction cosine matrix 63 | with no normalisation constraint. 64 | */ 65 | 66 | using AHRS_SensorErrorVector = UKF::StateVector< 67 | UKF::Field>, 68 | UKF::Field>, 69 | UKF::Field>, 70 | UKF::Field>, 71 | UKF::Field, 72 | UKF::Field 73 | >; 74 | 75 | namespace UKF { 76 | namespace Parameters { 77 | template <> constexpr real_t AlphaSquared = 1.0; 78 | template <> constexpr real_t Kappa = 3.0; 79 | } 80 | 81 | /* AHRS process model. */ 82 | template <> template <> 83 | AHRS_StateVector AHRS_StateVector::derivative<>() const { 84 | AHRS_StateVector output; 85 | 86 | /* Calculate change in attitude. */ 87 | UKF::Quaternion omega_q; 88 | omega_q.vec() = get_field() * 0.5; 89 | omega_q.w() = 0; 90 | output.set_field(omega_q.conjugate() * get_field()); 91 | 92 | /* Assume constant angular velocity. */ 93 | output.set_field(UKF::Vector<3>(0, 0, 0)); 94 | 95 | return output; 96 | } 97 | } 98 | 99 | using AHRS_MeasurementVector = UKF::DynamicMeasurementVector< 100 | UKF::Field>, 101 | UKF::Field>, 102 | UKF::Field> 103 | >; 104 | 105 | namespace UKF { 106 | /* 107 | This is the measurement model that's actually used in the filter, because 108 | it's the one which takes the parameter estimation filter state as an input. 109 | */ 110 | template <> template <> 111 | UKF::Vector<3> AHRS_MeasurementVector::expected_measurement 112 | ( 113 | const AHRS_StateVector& state, const AHRS_SensorErrorVector& input) { 114 | return input.get_field() + state.get_field() * UKF::Vector<3>(0, 0, -G_ACCEL); 115 | } 116 | 117 | template <> template <> 118 | UKF::Vector<3> AHRS_MeasurementVector::expected_measurement 119 | ( 120 | const AHRS_StateVector& state, const AHRS_SensorErrorVector& input) { 121 | return input.get_field() + state.get_field(); 122 | } 123 | 124 | template <> template <> 125 | UKF::Vector<3> AHRS_MeasurementVector::expected_measurement 126 | ( 127 | const AHRS_StateVector& state, const AHRS_SensorErrorVector& input) { 128 | return input.get_field().array() + input.get_field().array() * 129 | (state.get_field() * UKF::Vector<3>( 130 | input.get_field() * 131 | std::cos(std::atan(input.get_field())), 132 | 0.0, 133 | -input.get_field() * 134 | std::sin(std::atan(input.get_field())))).array(); 135 | } 136 | 137 | } 138 | 139 | using AHRS_Filter = UKF::SquareRootCore< 140 | AHRS_StateVector, 141 | AHRS_MeasurementVector, 142 | UKF::IntegratorHeun 143 | >; 144 | 145 | namespace UKF { 146 | /* 147 | AHRS parameter estimation filter process model. Since the evolution of sensor 148 | errors is by definition unpredictable, this does nothing. 149 | */ 150 | template <> template <> 151 | AHRS_SensorErrorVector AHRS_SensorErrorVector::derivative<>() const { 152 | return AHRS_SensorErrorVector::Zero(); 153 | } 154 | 155 | /* 156 | AHRS parameter estimation filter measurement model. These take in the current 157 | state estimate and sensor scale factor and bias estimates, and use them to 158 | calculate predicted measurements. 159 | These functions are just the same as the state measurement model, but with 160 | their arguments flipped. 161 | */ 162 | template <> template <> 163 | UKF::Vector<3> AHRS_MeasurementVector::expected_measurement 164 | ( 165 | const AHRS_SensorErrorVector& state, const AHRS_StateVector& input) { 166 | return state.get_field() + input.get_field() * UKF::Vector<3>(0, 0, -G_ACCEL); 167 | } 168 | 169 | template <> template <> 170 | UKF::Vector<3> AHRS_MeasurementVector::expected_measurement 171 | ( 172 | const AHRS_SensorErrorVector& state, const AHRS_StateVector& input) { 173 | return state.get_field() + input.get_field(); 174 | } 175 | 176 | template <> template <> 177 | UKF::Vector<3> AHRS_MeasurementVector::expected_measurement 178 | ( 179 | const AHRS_SensorErrorVector& state, const AHRS_StateVector& input) { 180 | return state.get_field().array() + state.get_field().array() * 181 | (input.get_field() * UKF::Vector<3>( 182 | state.get_field() * 183 | std::cos(std::atan(state.get_field())), 184 | 0.0, 185 | -state.get_field() * 186 | std::sin(std::atan(state.get_field())))).array(); 187 | } 188 | 189 | } 190 | 191 | /* Just use the Euler integrator since there's no process model. */ 192 | using AHRS_ParameterEstimationFilter = UKF::SquareRootParameterEstimationCore< 193 | AHRS_SensorErrorVector, 194 | AHRS_MeasurementVector 195 | >; 196 | 197 | static AHRS_Filter ahrs; 198 | static AHRS_ParameterEstimationFilter ahrs_errors; 199 | static AHRS_MeasurementVector meas; 200 | static UKF::Vector<3> acceleration; 201 | 202 | /* 203 | The following functions provide a ctypes-compatible interface for ease of 204 | testing. 205 | */ 206 | 207 | void ukf_init() { 208 | /* Initialise state vector and covariance. */ 209 | ahrs.state.set_field(UKF::Quaternion(1, 0, 0, 0)); 210 | ahrs.state.set_field(UKF::Vector<3>(0, 0, 0)); 211 | acceleration << UKF::Vector<3>(0, 0, 0); 212 | ahrs.root_covariance = AHRS_StateVector::CovarianceMatrix::Zero(); 213 | ahrs.root_covariance.diagonal() << 214 | 1e0, 1e0, 3.2e0, 215 | 1e-3 * UKF::Vector<3>::Ones(); 216 | 217 | /* Set measurement noise covariance. */ 218 | ahrs.measurement_root_covariance << 219 | 0.5 * UKF::Vector<3>::Ones(), 220 | 0.004 * UKF::Vector<3>::Ones(), 221 | 0.1 * UKF::Vector<3>::Ones(); 222 | 223 | /* Set process noise covariance. */ 224 | ahrs.process_noise_root_covariance = AHRS_StateVector::CovarianceMatrix::Zero(); 225 | ahrs.process_noise_root_covariance.diagonal() << 226 | 5e-5 * UKF::Vector<3>::Ones(), 227 | 5e-3 * UKF::Vector<3>::Ones(); 228 | 229 | /* Initialise scale factor and bias errors. */ 230 | ahrs_errors.state.set_field(UKF::Vector<3>(0, 0, 0)); 231 | ahrs_errors.state.set_field(UKF::Vector<3>(0, 0, 0)); 232 | ahrs_errors.state.set_field(UKF::Vector<3>(0, 0, 0)); 233 | ahrs_errors.state.set_field(UKF::Vector<3>(1, 1, 1)); 234 | ahrs_errors.state.set_field(MAG_NORM); 235 | ahrs_errors.state.set_field(0.0); 236 | 237 | /* Initialise scale factor and bias error covariance. */ 238 | ahrs_errors.root_covariance = AHRS_SensorErrorVector::CovarianceMatrix::Zero(); 239 | ahrs_errors.root_covariance.diagonal() << 240 | 0.8 * UKF::Vector<3>::Ones(), 241 | 0.02 * UKF::Vector<3>::Ones(), 242 | 5.0e-2 * UKF::Vector<3>::Ones(), 1.0e-1 * UKF::Vector<3>::Ones(), 243 | 0.4, 0.7; 244 | 245 | /* Set measurement noise covariance. */ 246 | ahrs_errors.measurement_root_covariance << ahrs.measurement_root_covariance; 247 | 248 | /* 249 | Set bias error process noise – this is derived from bias instability. 250 | 251 | Bias instability is actually characterised as a 1/f flicker noise rather 252 | than the white noise (which is what we're specifying using the process 253 | noise covariance), so these are tuned by hand to values which allow the 254 | filter to track biases over time, but not change too quickly. 255 | */ 256 | ahrs_errors.process_noise_root_covariance = AHRS_SensorErrorVector::CovarianceMatrix::Zero(); 257 | ahrs_errors.process_noise_root_covariance.diagonal() << 258 | 1e-5f * UKF::Vector<3>::Ones(), 259 | 1e-7f * UKF::Vector<3>::Ones(), 260 | 1e-5f * UKF::Vector<3>::Ones(), 1e-6f * UKF::Vector<3>::Ones(), 261 | 1e-7f, 1e-7f; 262 | } 263 | 264 | void ukf_set_attitude(real_t w, real_t x, real_t y, real_t z) { 265 | ahrs.state.set_field(UKF::Quaternion(w, x, y, z)); 266 | } 267 | 268 | void ukf_set_angular_velocity(real_t x, real_t y, real_t z) { 269 | ahrs.state.set_field(UKF::Vector<3>(x, y, z)); 270 | } 271 | 272 | void ukf_get_state(struct ukf_state_t *in) { 273 | in->attitude[0] = ahrs.state.get_field().x(); 274 | in->attitude[1] = ahrs.state.get_field().y(); 275 | in->attitude[2] = ahrs.state.get_field().z(); 276 | in->attitude[3] = ahrs.state.get_field().w(); 277 | in->angular_velocity[0] = ahrs.state.get_field()[0]; 278 | in->angular_velocity[1] = ahrs.state.get_field()[1]; 279 | in->angular_velocity[2] = ahrs.state.get_field()[2]; 280 | in->acceleration[0] = acceleration[0]; 281 | in->acceleration[1] = acceleration[1]; 282 | in->acceleration[2] = acceleration[2]; 283 | } 284 | 285 | void ukf_set_state(struct ukf_state_t *in) { 286 | ahrs.state.set_field( 287 | UKF::Quaternion(in->attitude[3], in->attitude[0], in->attitude[1], in->attitude[2])); 288 | ahrs.state.set_field( 289 | UKF::Vector<3>(in->angular_velocity[0], in->angular_velocity[1], in->angular_velocity[2])); 290 | } 291 | 292 | void ukf_get_state_covariance( 293 | real_t state_covariance[AHRS_StateVector::covariance_size()*AHRS_StateVector::covariance_size()]) { 294 | Eigen::Map covariance_map(state_covariance); 295 | covariance_map = ahrs.root_covariance * ahrs.root_covariance.transpose(); 296 | } 297 | 298 | void ukf_get_state_covariance_diagonal( 299 | real_t state_covariance_diagonal[AHRS_StateVector::covariance_size()]) { 300 | Eigen::Map> covariance_map(state_covariance_diagonal); 301 | covariance_map = (ahrs.root_covariance * ahrs.root_covariance.transpose()).diagonal(); 302 | } 303 | 304 | void ukf_get_state_error(struct ukf_state_error_t *in) { 305 | AHRS_StateVector::StateVectorDelta state_error; 306 | state_error = (ahrs.root_covariance * ahrs.root_covariance.transpose()).cwiseAbs().rowwise().sum().cwiseSqrt(); 307 | 308 | in->attitude[0] = state_error[0]; 309 | in->attitude[1] = state_error[1]; 310 | in->attitude[2] = state_error[2]; 311 | in->angular_velocity[0] = state_error[3]; 312 | in->angular_velocity[1] = state_error[4]; 313 | in->angular_velocity[2] = state_error[5]; 314 | } 315 | 316 | /* 317 | This assumes accelerometer, gyroscope and magnetometer measurements are 318 | present and in that order. 319 | */ 320 | void ukf_get_innovation(struct ukf_innovation_t *in) { 321 | in->accel[0] = ahrs.innovation[0]; 322 | in->accel[1] = ahrs.innovation[1]; 323 | in->accel[2] = ahrs.innovation[2]; 324 | in->gyro[0] = ahrs.innovation[3]; 325 | in->gyro[1] = ahrs.innovation[4]; 326 | in->gyro[2] = ahrs.innovation[5]; 327 | in->mag[0] = ahrs.innovation[6]; 328 | in->mag[1] = ahrs.innovation[7]; 329 | in->mag[2] = ahrs.innovation[8]; 330 | } 331 | 332 | void ukf_sensor_clear() { 333 | meas = AHRS_MeasurementVector(); 334 | } 335 | 336 | void ukf_sensor_set_accelerometer(real_t x, real_t y, real_t z) { 337 | meas.set_field(UKF::Vector<3>(x, y, z)); 338 | } 339 | 340 | void ukf_sensor_set_gyroscope(real_t x, real_t y, real_t z) { 341 | meas.set_field(UKF::Vector<3>(x, y, z)); 342 | } 343 | 344 | void ukf_sensor_set_magnetometer(real_t x, real_t y, real_t z) { 345 | meas.set_field(UKF::Vector<3>(x, y, z)); 346 | } 347 | 348 | void ukf_set_params(struct ukf_sensor_params_t *in) { 349 | ahrs.measurement_root_covariance << 350 | std::sqrt(in->accel_covariance[0]), std::sqrt(in->accel_covariance[1]), std::sqrt(in->accel_covariance[2]), 351 | std::sqrt(in->gyro_covariance[0]), std::sqrt(in->gyro_covariance[1]), std::sqrt(in->gyro_covariance[2]), 352 | std::sqrt(in->mag_covariance[0]), std::sqrt(in->mag_covariance[1]), std::sqrt(in->mag_covariance[2]); 353 | ahrs_errors.measurement_root_covariance << ahrs.measurement_root_covariance; 354 | } 355 | 356 | void ukf_iterate(float dt) { 357 | /* 358 | Split the parameter estimation filter into a priori and a posteriori 359 | steps, to reduce the CPU load each iteration. 360 | */ 361 | static int step = 0; 362 | 363 | switch(step++) { 364 | case 0: 365 | /* 366 | The time delta is not used by the parameter estimation filter, so 367 | there's no need to adjust it. 368 | */ 369 | ahrs_errors.a_priori_step(); 370 | ahrs_errors.innovation_step(meas, ahrs.state); 371 | break; 372 | case 1: 373 | ahrs_errors.a_posteriori_step(); 374 | 375 | /* Clip parameters to physically reasonable values. */ 376 | ahrs_errors.state.set_field( 377 | std::max(0.2f, std::min(0.7f, ahrs_errors.state.get_field()))); 378 | 379 | UKF::Vector<3> temp; 380 | temp = ahrs_errors.state.get_field(); 381 | temp[0] = std::max(real_t(-G_ACCEL/4.0), std::min(real_t(G_ACCEL/4.0), temp[0])); 382 | temp[1] = std::max(real_t(-G_ACCEL/4.0), std::min(real_t(G_ACCEL/4.0), temp[1])); 383 | temp[2] = std::max(real_t(-G_ACCEL/4.0), std::min(real_t(G_ACCEL/4.0), temp[2])); 384 | ahrs_errors.state.set_field(temp); 385 | 386 | temp = ahrs_errors.state.get_field(); 387 | temp[0] = std::max(real_t(0.5f), std::min(2.0f, temp[0])); 388 | temp[1] = std::max(real_t(0.5f), std::min(2.0f, temp[1])); 389 | temp[2] = std::max(real_t(0.5f), std::min(2.0f, temp[2])); 390 | ahrs_errors.state.set_field(temp); 391 | 392 | step = 0; 393 | break; 394 | } 395 | 396 | /* 397 | Do a normal iteration for the AHRS filter, with the current state of 398 | the parameter estimation filter as the measurement input. 399 | */ 400 | ahrs.a_priori_step(dt); 401 | ahrs.innovation_step(meas, ahrs_errors.state); 402 | ahrs.a_posteriori_step(); 403 | 404 | acceleration << meas.get_field() - ahrs_errors.state.get_field() - 405 | (ahrs.state.get_field() * UKF::Vector<3>(0, 0, -G_ACCEL)); 406 | } 407 | 408 | void ukf_set_process_noise(real_t process_noise_covariance[AHRS_StateVector::covariance_size()]) { 409 | Eigen::Map covariance_map(process_noise_covariance); 410 | ahrs.process_noise_root_covariance = AHRS_StateVector::CovarianceMatrix::Zero(); 411 | ahrs.process_noise_root_covariance.diagonal() << covariance_map; 412 | ahrs.process_noise_root_covariance = ahrs.process_noise_root_covariance.llt().matrixU(); 413 | } 414 | 415 | void ukf_get_parameters(struct ukf_sensor_errors_t *in) { 416 | in->accel_bias[0] = ahrs_errors.state.get_field()[0]; 417 | in->accel_bias[1] = ahrs_errors.state.get_field()[1]; 418 | in->accel_bias[2] = ahrs_errors.state.get_field()[2]; 419 | in->gyro_bias[0] = ahrs_errors.state.get_field()[0]; 420 | in->gyro_bias[1] = ahrs_errors.state.get_field()[1]; 421 | in->gyro_bias[2] = ahrs_errors.state.get_field()[2]; 422 | in->mag_bias[0] = ahrs_errors.state.get_field()[0]; 423 | in->mag_bias[1] = ahrs_errors.state.get_field()[1]; 424 | in->mag_bias[2] = ahrs_errors.state.get_field()[2]; 425 | in->mag_scale[0] = ahrs_errors.state.get_field()[0]; 426 | in->mag_scale[1] = ahrs_errors.state.get_field()[1]; 427 | in->mag_scale[2] = ahrs_errors.state.get_field()[2]; 428 | in->mag_field_norm = ahrs_errors.state.get_field(); 429 | in->mag_field_inclination = std::atan(ahrs_errors.state.get_field()); 430 | } 431 | 432 | void ukf_get_parameters_error(struct ukf_sensor_errors_t *in) { 433 | AHRS_SensorErrorVector::StateVectorDelta parameters_error; 434 | parameters_error = 435 | (ahrs_errors.root_covariance * ahrs_errors.root_covariance.transpose()).cwiseAbs().rowwise().sum().cwiseSqrt(); 436 | 437 | in->accel_bias[0] = parameters_error[0]; 438 | in->accel_bias[1] = parameters_error[1]; 439 | in->accel_bias[2] = parameters_error[2]; 440 | in->gyro_bias[0] = parameters_error[3]; 441 | in->gyro_bias[1] = parameters_error[4]; 442 | in->gyro_bias[2] = parameters_error[5]; 443 | in->mag_bias[0] = parameters_error[6]; 444 | in->mag_bias[1] = parameters_error[7]; 445 | in->mag_bias[2] = parameters_error[8]; 446 | in->mag_scale[0] = parameters_error[9]; 447 | in->mag_scale[1] = parameters_error[10]; 448 | in->mag_scale[2] = parameters_error[11]; 449 | in->mag_field_norm = parameters_error[12]; 450 | in->mag_field_inclination = parameters_error[13]; 451 | } 452 | 453 | uint32_t ukf_config_get_state_dim() { 454 | return AHRS_StateVector::covariance_size(); 455 | } 456 | 457 | uint32_t ukf_config_get_measurement_dim() { 458 | return AHRS_MeasurementVector::max_size(); 459 | } 460 | 461 | enum ukf_precision_t ukf_config_get_precision() { 462 | if(sizeof(real_t) == 8) { 463 | return UKF_PRECISION_DOUBLE; 464 | } else { 465 | return UKF_PRECISION_FLOAT; 466 | } 467 | } 468 | -------------------------------------------------------------------------------- /examples/ahrs/ahrs.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2016 Daniel Dyer 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #ifndef AHRS_H 24 | #define AHRS_H 25 | 26 | #ifdef __cplusplus 27 | extern "C" { 28 | #endif 29 | 30 | #define UKF_STATE_DIM 6 31 | #define UKF_MEASUREMENT_DIM 9 32 | 33 | /* 34 | Parameters for sensor model. 35 | 36 | Members are as follows: 37 | - accel_covariance: covariance of accelerometer readings (XYZ) 38 | - gyro_covariance: covariance of gyro readings (XYZ) 39 | - mag_covariance: covariance of magnetometer readings (XYZ) 40 | */ 41 | struct ukf_sensor_params_t { 42 | real_t accel_covariance[3]; 43 | real_t gyro_covariance[3]; 44 | real_t mag_covariance[3]; 45 | }; 46 | 47 | struct ukf_state_t { 48 | real_t attitude[4]; /* x, y, z, W */ 49 | real_t angular_velocity[3]; /* rolling (rad/s), pitching (rad/s), 50 | yawing (rad/s) */ 51 | real_t acceleration[3]; /* x, y, z (m/s^2) */ 52 | }; 53 | 54 | struct ukf_state_error_t { 55 | real_t attitude[3]; /* roll, pitch, yaw */ 56 | real_t angular_velocity[3]; /* rolling (rad/s), pitching (rad/s), 57 | yawing (rad/s) */ 58 | }; 59 | 60 | struct ukf_sensor_errors_t { 61 | real_t accel_bias[3]; 62 | real_t gyro_bias[3]; 63 | real_t mag_bias[3]; 64 | real_t mag_scale[3]; 65 | real_t mag_field_norm; 66 | real_t mag_field_inclination; 67 | }; 68 | 69 | struct ukf_innovation_t { 70 | real_t accel[3]; 71 | real_t gyro[3]; 72 | real_t mag[3]; 73 | }; 74 | 75 | void ukf_init(void); 76 | 77 | /* 78 | Note: W, x, y, z in the parameters for ukf_set_attitude differs to the stored 79 | attitude representation in struct ukf_state_t, which is x, y, z, W 80 | */ 81 | void ukf_set_attitude(real_t w, real_t x, real_t y, real_t z); 82 | void ukf_set_angular_velocity(real_t x, real_t y, real_t z); 83 | 84 | /* Functions for getting the state vector and covariance. */ 85 | void ukf_set_state(struct ukf_state_t *in); 86 | void ukf_get_state(struct ukf_state_t *in); 87 | void ukf_get_state_covariance( 88 | real_t state_covariance[UKF_STATE_DIM * UKF_STATE_DIM]); 89 | void ukf_get_state_covariance_diagonal( 90 | real_t state_covariance_diagonal[UKF_STATE_DIM]); 91 | void ukf_get_state_error(struct ukf_state_error_t *in); 92 | 93 | void ukf_get_innovation(struct ukf_innovation_t *in); 94 | 95 | /* 96 | Functions for setting sensor data. Before each frame, call the sensor_clear() 97 | function to clear old sensor data. 98 | */ 99 | void ukf_sensor_clear(void); 100 | void ukf_sensor_set_accelerometer(real_t x, real_t y, real_t z); 101 | void ukf_sensor_set_gyroscope(real_t x, real_t y, real_t z); 102 | void ukf_sensor_set_magnetometer(real_t x, real_t y, real_t z); 103 | 104 | /* 105 | UKF-related functions. 106 | */ 107 | void ukf_set_params(struct ukf_sensor_params_t *in); 108 | void ukf_set_process_noise(real_t process_noise_covariance[UKF_STATE_DIM]); 109 | /* dt is the time delta in seconds */ 110 | void ukf_iterate(float dt); 111 | 112 | /* 113 | Functions to get the current bias and scale factor estimates from the 114 | parameter estimation filter. 115 | */ 116 | void ukf_get_parameters(struct ukf_sensor_errors_t *in); 117 | void ukf_get_parameters_error(struct ukf_sensor_errors_t *in); 118 | 119 | /* 120 | Functions to access the compiled configuration 121 | */ 122 | enum ukf_precision_t { 123 | UKF_PRECISION_FLOAT = 0, 124 | UKF_PRECISION_DOUBLE = 1 125 | }; 126 | 127 | uint32_t ukf_config_get_state_dim(void); 128 | uint32_t ukf_config_get_measurement_dim(void); 129 | enum ukf_precision_t ukf_config_get_precision(void); 130 | 131 | #ifdef __cplusplus 132 | } 133 | #endif 134 | 135 | #endif 136 | -------------------------------------------------------------------------------- /examples/ahrs/python/ukf/__init__.py: -------------------------------------------------------------------------------- 1 | #Copyright (C) 2013 Daniel Dyer 2 | # 3 | #Permission is hereby granted, free of charge, to any person obtaining a copy 4 | #of this software and associated documentation files (the "Software"), to deal 5 | #in the Software without restriction, including without limitation the rights 6 | #to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | #copies of the Software, and to permit persons to whom the Software is 8 | #furnished to do so, subject to the following conditions: 9 | # 10 | #The above copyright notice and this permission notice shall be included in 11 | #all copies or substantial portions of the Software. 12 | # 13 | #THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | #IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | #FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | #AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | #LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | #OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | #SOFTWARE. 20 | 21 | import os 22 | from ctypes import * 23 | 24 | 25 | # Taken from c/cukf.h 26 | UKF_PRECISION_FLOAT = 0 27 | UKF_PRECISION_DOUBLE = 1 28 | 29 | 30 | state = None 31 | state_error = None 32 | innovation = None 33 | covariance = None 34 | parameters = None 35 | parameters_error = None 36 | 37 | 38 | # Internal globals, set during init 39 | _cukf = None 40 | _REAL_T = None 41 | 42 | 43 | # Internal classes, wrapping cukf structs directly 44 | class _SensorParams(Structure): 45 | pass 46 | 47 | 48 | class _State(Structure): 49 | def __repr__(self): 50 | fields = { 51 | "attitude": tuple(self.attitude), 52 | "angular_velocity": tuple(self.angular_velocity), 53 | "acceleration": tuple(self.angular_velocity) 54 | } 55 | return str(fields) 56 | 57 | class _StateError(Structure): 58 | def __repr__(self): 59 | fields = { 60 | "attitude": tuple(self.attitude), 61 | "angular_velocity": tuple(self.angular_velocity) 62 | } 63 | return str(fields) 64 | 65 | class _Innovation(Structure): 66 | def __repr__(self): 67 | fields = { 68 | "accel": tuple(self.accel), 69 | "gyro": tuple(self.gyro), 70 | "mag": tuple(self.mag) 71 | } 72 | return str(fields) 73 | 74 | class _Parameters(Structure): 75 | def __repr__(self): 76 | field = { 77 | "accel_bias": tuple(self.accel_bias), 78 | "gyro_bias": tuple(self.gyro_bias), 79 | "mag_bias": tuple(self.mag_bias), 80 | "mag_scale": tuple(self.mag_scale), 81 | "mag_field_norm": tuple(self.mag_field_norm), 82 | "mag_field_inclination": tuple(self.mag_field_inclination) 83 | } 84 | return std(fields) 85 | 86 | # Public interface 87 | def iterate(dt): 88 | global _cukf, state, state_error, innovation, parameters, parameters_error 89 | 90 | if not _cukf: 91 | raise RuntimeError("Please call ukf.init()") 92 | 93 | _cukf.ukf_set_state(state) 94 | _cukf.ukf_iterate(dt) 95 | _cukf.ukf_sensor_clear() 96 | _cukf.ukf_get_state(state) 97 | _cukf.ukf_get_state_error(state_error) 98 | _cukf.ukf_get_innovation(innovation) 99 | _cukf.ukf_get_parameters(parameters) 100 | _cukf.ukf_get_parameters_error(parameters_error) 101 | 102 | 103 | def set_sensors(accelerometer=None, gyroscope=None, magnetometer=None): 104 | if accelerometer is not None: 105 | _cukf.ukf_sensor_set_accelerometer(*accelerometer) 106 | if gyroscope is not None: 107 | _cukf.ukf_sensor_set_gyroscope(*gyroscope) 108 | if magnetometer is not None: 109 | _cukf.ukf_sensor_set_magnetometer(*magnetometer) 110 | 111 | 112 | def configure_sensors(accelerometer_covariance=None, 113 | gyroscope_covariance=None, magnetometer_covariance=None): 114 | params = _SensorParams() 115 | 116 | if getattr(accelerometer_covariance, '__iter__', False): 117 | params.accel_covariance = accelerometer_covariance 118 | elif accelerometer_covariance is not None: 119 | params.accel_covariance = (accelerometer_covariance, ) * 3 120 | else: 121 | params.accel_covariance = (1.0, 1.0, 1.0) 122 | 123 | if getattr(gyroscope_covariance, '__iter__', False): 124 | params.gyro_covariance = gyroscope_covariance 125 | elif gyroscope_covariance is not None: 126 | params.gyro_covariance = (gyroscope_covariance, ) * 3 127 | else: 128 | params.gyro_covariance = (1.0, 1.0, 1.0) 129 | 130 | if getattr(magnetometer_covariance, '__iter__', False): 131 | params.mag_covariance = magnetometer_covariance 132 | elif magnetometer_covariance is not None: 133 | params.mag_covariance = (magnetometer_covariance, ) * 3 134 | else: 135 | params.mag_covariance = (1.0, 1.0, 1.0) 136 | 137 | _cukf.ukf_set_params(params) 138 | 139 | 140 | def configure_process_noise(process_noise_covariance): 141 | _cukf.ukf_set_process_noise((_REAL_T * 6)(*process_noise_covariance)) 142 | 143 | 144 | def init(): 145 | global _cukf, _REAL_T, state, state_error, innovation, parameters, parameters_error 146 | 147 | lib = os.path.join(os.path.dirname(__file__), "libahrs.dylib") 148 | _cukf = cdll.LoadLibrary(lib) 149 | 150 | _cukf.ukf_init.argtypes = [] 151 | _cukf.ukf_init.restype = None 152 | 153 | _cukf.ukf_config_get_precision.argtypes = [] 154 | _cukf.ukf_config_get_precision.restype = c_long 155 | 156 | _cukf.ukf_config_get_state_dim.argtypes = [] 157 | _cukf.ukf_config_get_state_dim.restype = c_long 158 | 159 | _cukf.ukf_config_get_measurement_dim.argtypes = [] 160 | _cukf.ukf_config_get_measurement_dim.restype = c_long 161 | 162 | _PRECISION = _cukf.ukf_config_get_precision() 163 | _REAL_T = c_double if _PRECISION == UKF_PRECISION_DOUBLE else c_float 164 | _STATE_DIM = _cukf.ukf_config_get_state_dim() 165 | _MEASUREMENT_DIM = _cukf.ukf_config_get_measurement_dim() 166 | 167 | _SensorParams._fields_ = [ 168 | ("accel_covariance", _REAL_T * 3), 169 | ("gyro_covariance", _REAL_T * 3), 170 | ("mag_covariance", _REAL_T * 3) 171 | ] 172 | 173 | _State._fields_ = [ 174 | ("attitude", _REAL_T * 4), 175 | ("angular_velocity", _REAL_T * 3), 176 | ("acceleration", _REAL_T * 3) 177 | ] 178 | 179 | _StateError._fields_ = [ 180 | ("attitude", _REAL_T * 3), 181 | ("angular_velocity", _REAL_T * 3) 182 | ] 183 | 184 | _Innovation._fields_ = [ 185 | ("accel", _REAL_T * 3), 186 | ("gyro", _REAL_T * 3), 187 | ("mag", _REAL_T * 3) 188 | ] 189 | 190 | _Parameters._fields_ = [ 191 | ("accel_bias", _REAL_T * 3), 192 | ("gyro_bias", _REAL_T * 3), 193 | ("mag_bias", _REAL_T * 3), 194 | ("mag_scale", _REAL_T * 3), 195 | ("mag_field_norm", _REAL_T), 196 | ("mag_field_inclination", _REAL_T), 197 | ] 198 | 199 | # Set up the function prototypes 200 | _cukf.ukf_set_attitude.argtypes = [_REAL_T, _REAL_T, _REAL_T, _REAL_T] 201 | _cukf.ukf_set_attitude.restype = None 202 | 203 | _cukf.ukf_set_angular_velocity.argtypes = [_REAL_T, _REAL_T, _REAL_T] 204 | _cukf.ukf_set_angular_velocity.restype = None 205 | 206 | _cukf.ukf_get_state.argtypes = [POINTER(_State)] 207 | _cukf.ukf_get_state.restype = None 208 | 209 | _cukf.ukf_set_state.argtypes = [POINTER(_State)] 210 | _cukf.ukf_set_state.restype = None 211 | 212 | _cukf.ukf_get_state_error.argtypes = [POINTER(_StateError)] 213 | _cukf.ukf_get_state_error.restype = None 214 | 215 | _cukf.ukf_get_innovation.argtypes = [POINTER(_Innovation)] 216 | _cukf.ukf_get_innovation.restype = None 217 | 218 | _cukf.ukf_get_state_covariance.argtypes = [ 219 | POINTER(_REAL_T * (_STATE_DIM**2))] 220 | _cukf.ukf_get_state_covariance.restype = None 221 | 222 | _cukf.ukf_sensor_clear.argtypes = [] 223 | _cukf.ukf_sensor_clear.restype = None 224 | 225 | _cukf.ukf_sensor_set_accelerometer.argtypes = [_REAL_T, _REAL_T, _REAL_T] 226 | _cukf.ukf_sensor_set_accelerometer.restype = None 227 | 228 | _cukf.ukf_sensor_set_gyroscope.argtypes = [_REAL_T, _REAL_T, _REAL_T] 229 | _cukf.ukf_sensor_set_gyroscope.restype = None 230 | 231 | _cukf.ukf_sensor_set_magnetometer.argtypes = [_REAL_T, _REAL_T, _REAL_T] 232 | _cukf.ukf_sensor_set_magnetometer.restype = None 233 | 234 | _cukf.ukf_set_params.argtypes = [POINTER(_SensorParams)] 235 | _cukf.ukf_set_params.restype = None 236 | 237 | _cukf.ukf_iterate.argtypes = [c_float] 238 | _cukf.ukf_iterate.restype = None 239 | 240 | _cukf.ukf_set_process_noise.argtypes = [POINTER(_REAL_T * _STATE_DIM)] 241 | _cukf.ukf_set_process_noise.restype = None 242 | 243 | _cukf.ukf_get_parameters.argtypes = [POINTER(_Parameters)] 244 | _cukf.ukf_get_parameters.restype = None 245 | 246 | _cukf.ukf_get_parameters_error.argtypes = [POINTER(_Parameters)] 247 | _cukf.ukf_get_parameters_error.restype = None 248 | 249 | # Initialize the library 250 | _cukf.ukf_init() 251 | 252 | # Set up the state 253 | state = _State() 254 | _cukf.ukf_get_state(state) 255 | 256 | # Set up the state errors 257 | state_error = _StateError() 258 | _cukf.ukf_get_state_error(state_error) 259 | 260 | # Set up the innovation 261 | innovation = _Innovation() 262 | 263 | # Set up the parameters 264 | parameters = _Parameters() 265 | _cukf.ukf_get_parameters(parameters) 266 | 267 | # Set up the parameter errors 268 | parameters_error = _Parameters() 269 | _cukf.ukf_get_parameters_error(parameters_error) 270 | -------------------------------------------------------------------------------- /examples/sfwa_ukf/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8.4 FATAL_ERROR) 2 | PROJECT(sfwa_ukf) 3 | 4 | # Disable dynamic memory allocation in Eigen 5 | ADD_DEFINITIONS(-DEIGEN_NO_MALLOC -DUKF_DOUBLE_PRECISION) 6 | 7 | INCLUDE_DIRECTORIES(../../include ${eigen_dir}) 8 | 9 | cmake_policy(SET CMP0042 NEW) 10 | 11 | ADD_LIBRARY(sfwa_ukf SHARED cukf.cpp) 12 | SET_TARGET_PROPERTIES(sfwa_ukf PROPERTIES OUTPUT_NAME cukf) -------------------------------------------------------------------------------- /examples/sfwa_ukf/cukf.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "UKF/Types.h" 5 | #include "UKF/Integrator.h" 6 | #include "UKF/StateVector.h" 7 | #include "UKF/MeasurementVector.h" 8 | #include "UKF/Core.h" 9 | #include "cukf.h" 10 | 11 | /* 12 | This file implements the same UKF as was used for the 2014 UAV Challenge. 13 | There are a number of efficiency and performance improvements that could be 14 | made with the new library structure, but the main purpose of this is for 15 | direct comparison of performance of the new library against the old 16 | implementation. 17 | */ 18 | 19 | enum SFWA_States { 20 | LatLon, 21 | Altitude, 22 | Velocity, 23 | Acceleration, 24 | Attitude, 25 | AngularVelocity, 26 | AngularAcceleration, 27 | WindVelocity, 28 | GyroBias 29 | }; 30 | 31 | using SFWA_StateVector = UKF::StateVector< 32 | UKF::Field>, /* Latitude and longitude (rad) */ 33 | UKF::Field, /* Altitude above the WGS84 ellipsoid (m) */ 34 | UKF::Field>, /* Velocity (NED frame, m/s) */ 35 | UKF::Field>, /* Acceleration (body frame, m/s^2) */ 36 | UKF::Field, /* Attitude as a quaternion (NED frame to body frame) */ 37 | UKF::Field>, /* Angular velocity (body frame, rad/s) */ 38 | UKF::Field>, /* Angular acceleration (body frame, rad/s^2) */ 39 | UKF::Field>, /* Wind velocity (NED frame, m/s) */ 40 | UKF::Field> /* Gyro bias (body frame, rad/s) */ 41 | >; 42 | 43 | /* WGS84 reference ellipsoid constants. */ 44 | #define WGS84_A (6378137.0) 45 | #define WGS84_B (6356752.314245) 46 | #define WGS84_A2 (WGS84_A*WGS84_A) 47 | #define WGS84_B2 (WGS84_B*WGS84_B) 48 | #define WGS84_AB2 (WGS84_A2*WGS84_B2) 49 | 50 | #define G_ACCEL (9.80665) 51 | #define RHO (1.225) 52 | 53 | /* SFWA vehicle dynamics model. Not used for this comparison. */ 54 | UKF::Vector<6> x8_dynamics_model(const SFWA_StateVector &state, const UKF::Vector<3> &control) { 55 | UKF::Vector<6> output; 56 | 57 | /* Cache state data for convenience */ 58 | UKF::Quaternion attitude = state.get_field(); 59 | real_t yaw_rate = state.get_field()[2], 60 | pitch_rate = state.get_field()[1], 61 | roll_rate = state.get_field()[0]; 62 | 63 | /* External axes */ 64 | UKF::Vector<3> airflow = attitude * (state.get_field() - state.get_field()); 65 | 66 | /* 67 | Calculate axial airflow 68 | */ 69 | real_t airflow_x2, airflow_y2, airflow_z2, airflow_v2; 70 | airflow_x2 = airflow[0]*airflow[0]; 71 | airflow_y2 = airflow[1]*airflow[1]; 72 | airflow_z2 = airflow[2]*airflow[2]; 73 | airflow_v2 = airflow_x2 + airflow_y2 + airflow_z2; 74 | 75 | /* 76 | Determine motor thrust and torque. 77 | */ 78 | real_t rpm = control[0] * 12000.0, thrust, 79 | ve2 = (0.0025 * 0.0025) * rpm * rpm; 80 | /* 1 / 3.8kg times area * density of air */ 81 | thrust = (ve2 - airflow_v2) * 82 | (0.26315789473684 * 0.5 * RHO * 0.02); 83 | 84 | /* 85 | Calculate airflow in the horizontal and vertical planes, as well as 86 | pressure 87 | */ 88 | real_t v_inv, vertical_v, vertical_v_inv, qbar; 89 | 90 | qbar = (RHO * 0.5) * airflow_v2; 91 | v_inv = 1.0 / std::sqrt(std::max(1.0, airflow_v2)); 92 | 93 | vertical_v = std::sqrt(airflow_x2 + airflow_z2); 94 | vertical_v_inv = 1.0 / std::max(1.0, vertical_v); 95 | 96 | /* Work out sin/cos of alpha and beta */ 97 | real_t sin_alpha, cos_alpha, sin_beta, cos_beta, sin_cos_alpha; 98 | 99 | sin_beta = airflow[1] * v_inv; 100 | cos_beta = vertical_v * v_inv; 101 | 102 | sin_alpha = -airflow[2] * vertical_v_inv; 103 | cos_alpha = -airflow[0] * vertical_v_inv; 104 | 105 | sin_cos_alpha = sin_alpha * cos_alpha; 106 | 107 | /* Work out aerodynamic forces in wind frame */ 108 | real_t lift, drag, side_force; 109 | 110 | /* 0.26315789473684 is the reciprocal of mass (3.8kg) */ 111 | lift = (qbar * 0.26315789473684) * (0.8 * sin_cos_alpha + 0.18); 112 | drag = (qbar * 0.26315789473684) * 113 | (0.05 + 0.7 * sin_alpha * sin_alpha); 114 | side_force = (qbar * 0.26315789473684) * 0.2 * sin_beta * cos_beta; 115 | 116 | /* Convert aerodynamic forces from wind frame to body frame */ 117 | real_t x_aero_f = lift * sin_alpha - drag * cos_alpha - 118 | side_force * sin_beta, 119 | z_aero_f = lift * cos_alpha + drag * sin_alpha, 120 | y_aero_f = side_force * cos_beta; 121 | 122 | output.segment<3>(0) << UKF::Vector<3>(x_aero_f + thrust, y_aero_f, -z_aero_f) + 123 | (attitude * UKF::Vector<3>(0, 0, G_ACCEL)); 124 | 125 | /* Determine moments */ 126 | real_t pitch_moment, yaw_moment, roll_moment, 127 | left_aileron = control[1] - 0.5, right_aileron = control[2] - 0.5; 128 | pitch_moment = 0.0 - 0.0 * sin_alpha - 0.0 * pitch_rate - 129 | 0.1 * (left_aileron + right_aileron) * vertical_v * 0.1; 130 | roll_moment = 0.05 * sin_beta - 0.1 * roll_rate + 131 | 0.15 * (left_aileron - right_aileron) * vertical_v * 0.1; 132 | yaw_moment = -0.02 * sin_beta - 0.05 * yaw_rate - 133 | 0.02 * (std::abs(left_aileron) + std::abs(right_aileron)) * 134 | vertical_v * 0.1; 135 | pitch_moment *= qbar; 136 | roll_moment *= qbar; 137 | yaw_moment *= qbar; 138 | 139 | /* 140 | Calculate angular acceleration (tau / inertia tensor). 141 | Inertia tensor is: 142 | 0.3 0 -0.0334 143 | 0 0.17 0 144 | -0.0334 0 0.405 145 | So inverse is: 146 | 3.36422 0 0.277444 147 | 0 5.88235 0 148 | 0.277444 0 2.49202 149 | */ 150 | output.segment<3>(3) << UKF::Vector<3>( 151 | (3.364222 * roll_moment + 0.27744448 * yaw_moment), 152 | 10.8823528 * pitch_moment, 153 | (0.27744448 * roll_moment + 2.4920163 * yaw_moment)); 154 | 155 | return output; 156 | } 157 | 158 | namespace UKF { 159 | 160 | /* SFWA state vector process model. */ 161 | template <> template <> 162 | SFWA_StateVector SFWA_StateVector::derivative<>() const { 163 | SFWA_StateVector output; 164 | 165 | /* Calculate the normal and meridional radii of curvature. */ 166 | real_t lat = get_field()[0]; 167 | real_t tempA = WGS84_A*std::cos(lat), tempB = WGS84_B*std::sin(lat), 168 | temp = tempA * tempA + tempB * tempB, 169 | temp_sqrt = std::sqrt(temp); 170 | real_t M = WGS84_AB2 / (temp_sqrt * temp); 171 | real_t N = WGS84_A2 / temp_sqrt; 172 | 173 | /* 174 | Calculate change in position. Using the small angle approximation, this 175 | becomes very simple – no trig required for latitude derivative, and one 176 | cosine function for longitude derivative. 177 | */ 178 | UKF::Vector<3> vel = get_field(); 179 | output.set_field(Vector<2>( 180 | vel[0] / (M + get_field()), 181 | (vel[1] / (N + get_field())) * std::cos(lat))); 182 | output.set_field(-vel[2]); 183 | 184 | /* Calculate change in velocity. */ 185 | output.set_field(get_field().conjugate() * get_field()); 186 | 187 | /* Change in linear acceleration is zero. */ 188 | output.set_field(UKF::Vector<3>(0, 0, 0)); 189 | 190 | /* Calculate change in attitude. */ 191 | UKF::Quaternion omega_q; 192 | omega_q.vec() = get_field() * 0.5; 193 | omega_q.w() = 0; 194 | output.set_field(omega_q.conjugate() * get_field()); 195 | 196 | /* Calculate change in angular velocity (just angular acceleration). */ 197 | output.set_field(get_field()); 198 | 199 | /* Change in angular acceleration is zero. */ 200 | output.set_field(UKF::Vector<3>(0, 0, 0)); 201 | 202 | /* Change in wind velocity is zero. */ 203 | output.set_field(UKF::Vector<3>(0, 0, 0)); 204 | 205 | /* Change in gyro bias is zero. */ 206 | output.set_field(UKF::Vector<3>(0, 0, 0)); 207 | 208 | return output; 209 | } 210 | 211 | } 212 | 213 | enum SFWA_Measurements { 214 | Accelerometer, 215 | Gyroscope, 216 | Magnetometer, 217 | GPS_Position, 218 | GPS_Velocity, 219 | Airspeed, 220 | PressureAltitude 221 | }; 222 | 223 | using SFWA_MeasurementVector = UKF::DynamicMeasurementVector< 224 | UKF::Field>, 225 | UKF::Field>, 226 | UKF::Field>, 227 | UKF::Field>, 228 | UKF::Field>, 229 | UKF::Field, 230 | UKF::Field 231 | >; 232 | 233 | /* 234 | Hard code the magnetic field vector to the WMM value for 235 | -37.954690, 145.237575 for the purposes of this comparison. Value is in 236 | microtesla. 237 | */ 238 | static UKF::Vector<3> local_mag_field = UKF::Vector<3>(21.2584, 4.4306, -55.9677); 239 | 240 | using SFWA_UKF = UKF::Core< 241 | SFWA_StateVector, 242 | SFWA_MeasurementVector, 243 | UKF::IntegratorRK4 244 | >; 245 | 246 | namespace UKF { 247 | /* SFWA measurement model. */ 248 | template <> template <> 249 | UKF::Vector<3> SFWA_MeasurementVector::expected_measurement 250 | (const SFWA_StateVector& state) { 251 | return state.get_field() + state.get_field() * UKF::Vector<3>(0, 0, -G_ACCEL); 252 | } 253 | 254 | template <> template <> 255 | UKF::Vector<3> SFWA_MeasurementVector::expected_measurement 256 | (const SFWA_StateVector& state) { 257 | return state.get_field() + state.get_field(); 258 | } 259 | 260 | template <> template <> 261 | UKF::Vector<3> SFWA_MeasurementVector::expected_measurement 262 | (const SFWA_StateVector& state) { 263 | return state.get_field() * local_mag_field; 264 | } 265 | 266 | template <> template <> 267 | UKF::Vector<3> SFWA_MeasurementVector::expected_measurement 268 | (const SFWA_StateVector& state) { 269 | return UKF::Vector<3>(state.get_field()[0], state.get_field()[1], state.get_field()); 270 | } 271 | 272 | template <> template <> 273 | UKF::Vector<3> SFWA_MeasurementVector::expected_measurement 274 | (const SFWA_StateVector& state) { 275 | return state.get_field(); 276 | } 277 | 278 | template <> template <> 279 | real_t SFWA_MeasurementVector::expected_measurement 280 | (const SFWA_StateVector& state) { 281 | return (state.get_field() * (state.get_field() - state.get_field()))[0]; 282 | } 283 | 284 | template <> template <> 285 | real_t SFWA_MeasurementVector::expected_measurement 286 | (const SFWA_StateVector& state) { 287 | return state.get_field(); 288 | } 289 | 290 | } 291 | static SFWA_UKF ukf; 292 | static SFWA_MeasurementVector meas; 293 | 294 | /* 295 | The following functions provide a ctypes-compatible interface for ease of 296 | testing. 297 | */ 298 | 299 | void ukf_init() { 300 | ukf.state.set_field(UKF::Vector<2>(-0.662434, 2.534874)); 301 | ukf.state.set_field(0); 302 | ukf.state.set_field(UKF::Vector<3>(0, 0, 0)); 303 | ukf.state.set_field(UKF::Vector<3>(0, 0, 0)); 304 | ukf.state.set_field(UKF::Quaternion(1, 0, 0, 0)); 305 | ukf.state.set_field(UKF::Vector<3>(0, 0, 0)); 306 | ukf.state.set_field(UKF::Vector<3>(0, 0, 0)); 307 | ukf.state.set_field(UKF::Vector<3>(0, 0, 0)); 308 | ukf.state.set_field(UKF::Vector<3>(0, 0, 0)); 309 | ukf.covariance = SFWA_StateVector::CovarianceMatrix::Zero(); 310 | ukf.covariance.diagonal() << 311 | 1, 1, 10000, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 10, 10, 10, 312 | 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 100, 100, 100, 0.01, 0.01, 0.01; 313 | ukf.measurement_covariance <<1, 1, 1, 0.01, 0.01, 0.01, 10, 10, 10, 1e-20, 1e-20, 1, 1, 1, 1, 1, 1; 314 | 315 | local_mag_field = UKF::Vector<3>(21.2584, 4.4306, -55.9677); 316 | 317 | ukf.process_noise_covariance = SFWA_StateVector::CovarianceMatrix::Zero(); 318 | ukf.process_noise_covariance.diagonal() << 319 | 1e-17, 1e-17, 1e-4, /* lat, lon, alt */ 320 | 2e-3, 2e-3, 2e-3, /* velocity N, E, D */ 321 | 2e-2, 2e-2, 2e-2, /* acceleration x, y, z */ 322 | 7e-8, 7e-8, 7e-8, /* attitude roll, pitch, yaw */ 323 | 1e-3, 1e-3, 1e-3, /* angular velocity roll, pitch, yaw */ 324 | 1e-3, 1e-3, 1e-3, /* angular acceleration roll, pitch, yaw */ 325 | 1e-7, 1e-7, 1e-7, /* wind velocity N, E, D -- NOTE: from FCS armed mode */ 326 | 1e-12, 1e-12, 1e-12; /* gyro bias x, y, z -- NOTE: from FCS armed mode */ 327 | } 328 | 329 | void ukf_set_position(real_t lat, real_t lon, real_t alt) { 330 | ukf.state.set_field(UKF::Vector<2>(lat, lon)); 331 | ukf.state.set_field(alt); 332 | } 333 | 334 | void ukf_set_velocity(real_t x, real_t y, real_t z) { 335 | ukf.state.set_field(UKF::Vector<3>(x, y, z)); 336 | } 337 | 338 | void ukf_set_acceleration(real_t x, real_t y, real_t z) { 339 | ukf.state.set_field(UKF::Vector<3>(x, y, z)); 340 | } 341 | 342 | void ukf_set_attitude(real_t w, real_t x, real_t y, real_t z) { 343 | ukf.state.set_field(UKF::Quaternion(w, x, y, z)); 344 | } 345 | 346 | void ukf_set_angular_velocity(real_t x, real_t y, real_t z) { 347 | ukf.state.set_field(UKF::Vector<3>(x, y, z)); 348 | } 349 | 350 | void ukf_set_angular_acceleration(real_t x, real_t y, real_t z) { 351 | ukf.state.set_field(UKF::Vector<3>(x, y, z)); 352 | } 353 | 354 | void ukf_set_wind_velocity(real_t x, real_t y, real_t z) { 355 | ukf.state.set_field(UKF::Vector<3>(x, y, z)); 356 | } 357 | 358 | void ukf_set_gyro_bias(real_t x, real_t y, real_t z) { 359 | ukf.state.set_field(UKF::Vector<3>(x, y, z)); 360 | } 361 | 362 | void ukf_get_state(struct ukf_state_t *in) { 363 | in->position[0] = ukf.state.get_field()[0]; 364 | in->position[1] = ukf.state.get_field()[1]; 365 | in->position[2] = ukf.state.get_field(); 366 | in->velocity[0] = ukf.state.get_field()[0]; 367 | in->velocity[1] = ukf.state.get_field()[1]; 368 | in->velocity[2] = ukf.state.get_field()[2]; 369 | in->acceleration[0] = ukf.state.get_field()[0]; 370 | in->acceleration[1] = ukf.state.get_field()[1]; 371 | in->acceleration[2] = ukf.state.get_field()[2]; 372 | in->attitude[0] = ukf.state.get_field().x(); 373 | in->attitude[1] = ukf.state.get_field().y(); 374 | in->attitude[2] = ukf.state.get_field().z(); 375 | in->attitude[3] = ukf.state.get_field().w(); 376 | in->angular_velocity[0] = ukf.state.get_field()[0]; 377 | in->angular_velocity[1] = ukf.state.get_field()[1]; 378 | in->angular_velocity[2] = ukf.state.get_field()[2]; 379 | in->angular_acceleration[0] = ukf.state.get_field()[0]; 380 | in->angular_acceleration[1] = ukf.state.get_field()[1]; 381 | in->angular_acceleration[2] = ukf.state.get_field()[2]; 382 | in->wind_velocity[0] = ukf.state.get_field()[0]; 383 | in->wind_velocity[1] = ukf.state.get_field()[1]; 384 | in->wind_velocity[2] = ukf.state.get_field()[2]; 385 | in->gyro_bias[0] = ukf.state.get_field()[0]; 386 | in->gyro_bias[1] = ukf.state.get_field()[1]; 387 | in->gyro_bias[2] = ukf.state.get_field()[2]; 388 | } 389 | 390 | void ukf_set_state(struct ukf_state_t *in) { 391 | ukf.state.set_field(UKF::Vector<2>(in->position[0], in->position[1])); 392 | ukf.state.set_field(in->position[2]); 393 | ukf.state.set_field( 394 | UKF::Vector<3>(in->velocity[0], in->velocity[1], in->velocity[2])); 395 | ukf.state.set_field( 396 | UKF::Vector<3>(in->acceleration[0], in->acceleration[1], in->acceleration[2])); 397 | ukf.state.set_field( 398 | UKF::Quaternion(in->attitude[3], in->attitude[0], in->attitude[1], in->attitude[2])); 399 | ukf.state.set_field( 400 | UKF::Vector<3>(in->angular_velocity[0], in->angular_velocity[1], in->angular_velocity[2])); 401 | ukf.state.set_field( 402 | UKF::Vector<3>(in->angular_acceleration[0], in->angular_acceleration[1], in->angular_acceleration[2])); 403 | ukf.state.set_field( 404 | UKF::Vector<3>(in->wind_velocity[0], in->wind_velocity[1], in->wind_velocity[2])); 405 | ukf.state.set_field( 406 | UKF::Vector<3>(in->gyro_bias[0], in->gyro_bias[1], in->gyro_bias[2])); 407 | } 408 | 409 | void ukf_get_state_covariance( 410 | real_t state_covariance[SFWA_StateVector::covariance_size()*SFWA_StateVector::covariance_size()]) { 411 | Eigen::Map covariance_map(state_covariance); 412 | covariance_map = ukf.covariance; 413 | } 414 | 415 | void ukf_get_state_covariance_diagonal( 416 | real_t state_covariance_diagonal[SFWA_StateVector::covariance_size()]) { 417 | Eigen::Map> covariance_map(state_covariance_diagonal); 418 | covariance_map = ukf.covariance.diagonal(); 419 | } 420 | 421 | void ukf_get_state_error(real_t state_error[SFWA_StateVector::covariance_size()]) { 422 | Eigen::Map error_map(state_error); 423 | error_map = ukf.covariance.cwiseAbs().rowwise().sum().cwiseSqrt(); 424 | } 425 | 426 | void ukf_sensor_clear() { 427 | meas = SFWA_MeasurementVector(); 428 | } 429 | 430 | void ukf_sensor_set_accelerometer(real_t x, real_t y, real_t z) { 431 | meas.set_field(UKF::Vector<3>(x, y, z)); 432 | } 433 | 434 | void ukf_sensor_set_gyroscope(real_t x, real_t y, real_t z) { 435 | meas.set_field(UKF::Vector<3>(x, y, z)); 436 | } 437 | 438 | void ukf_sensor_set_magnetometer(real_t x, real_t y, real_t z) { 439 | meas.set_field(UKF::Vector<3>(x, y, z)); 440 | } 441 | 442 | void ukf_sensor_set_gps_position(real_t lat, real_t lon, real_t alt) { 443 | meas.set_field(UKF::Vector<3>(lat, lon, alt)); 444 | } 445 | 446 | void ukf_sensor_set_gps_velocity(real_t x, real_t y, real_t z) { 447 | meas.set_field(UKF::Vector<3>(x, y, z)); 448 | } 449 | 450 | void ukf_sensor_set_pitot_tas(real_t tas) { 451 | meas.set_field(tas); 452 | } 453 | 454 | void ukf_sensor_set_barometer_amsl(real_t amsl) { 455 | meas.set_field(amsl); 456 | } 457 | 458 | void ukf_set_params(struct ukf_ioboard_params_t *in) { 459 | local_mag_field << in->mag_field[0], in->mag_field[1], in->mag_field[2]; 460 | ukf.measurement_covariance << 461 | in->accel_covariance[0], in->accel_covariance[1], in->accel_covariance[2], 462 | in->gyro_covariance[0], in->gyro_covariance[1], in->gyro_covariance[2], 463 | in->mag_covariance[0], in->mag_covariance[1], in->mag_covariance[2], 464 | in->gps_position_covariance[0], in->gps_position_covariance[1], in->gps_position_covariance[2], 465 | in->gps_velocity_covariance[0], in->gps_velocity_covariance[1], in->gps_velocity_covariance[2], 466 | in->pitot_covariance, 467 | in->barometer_amsl_covariance; 468 | } 469 | 470 | void ukf_choose_dynamics(enum ukf_model_t t) { 471 | 472 | } 473 | 474 | void ukf_set_custom_dynamics_model(ukf_model_function_t func) { 475 | 476 | } 477 | 478 | void ukf_iterate(float dt, real_t control_vector[UKF_CONTROL_DIM]) { 479 | ukf.step(dt, meas); 480 | } 481 | 482 | void ukf_set_process_noise(real_t process_noise_covariance[SFWA_StateVector::covariance_size()]) { 483 | Eigen::Map covariance_map(process_noise_covariance); 484 | ukf.process_noise_covariance = SFWA_StateVector::CovarianceMatrix::Zero(); 485 | ukf.process_noise_covariance.diagonal() << covariance_map; 486 | } 487 | 488 | uint32_t ukf_config_get_state_dim() { 489 | return SFWA_StateVector::covariance_size(); 490 | } 491 | 492 | uint32_t ukf_config_get_measurement_dim() { 493 | return SFWA_MeasurementVector::max_size(); 494 | } 495 | 496 | uint32_t ukf_config_get_control_dim() { 497 | return UKF_CONTROL_DIM; 498 | } 499 | 500 | enum ukf_precision_t ukf_config_get_precision() { 501 | return UKF_PRECISION_DOUBLE; 502 | } 503 | -------------------------------------------------------------------------------- /examples/sfwa_ukf/cukf.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2016 Daniel Dyer 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #ifndef INTERFACE_H 24 | #define INTERFACE_H 25 | 26 | #ifdef __cplusplus 27 | extern "C" { 28 | #endif 29 | 30 | #define UKF_STATE_DIM 24 31 | #define UKF_MEASUREMENT_DIM 17 32 | #define UKF_CONTROL_DIM 4 33 | 34 | /* Dynamics model types. */ 35 | enum ukf_model_t { 36 | UKF_MODEL_NONE = 0, 37 | UKF_MODEL_CENTRIPETAL = 1, 38 | UKF_MODEL_CUSTOM = 2, 39 | UKF_MODEL_X8 = 3 40 | }; 41 | 42 | /* 43 | Parameters for ioboard sensor model. 44 | 45 | Members are as follows: 46 | - accel_orientation: Quaternion from body frame to accelerometer frame. 47 | - accel_offset: Accelerometer offset from CoG in metres. 48 | - gyro_orientation: Quaternion from body frame to gyroscope frame. 49 | - mag_orientation: Quaternion from body frame to magnetometer frame. 50 | - mag_field: Expected magnetic field in NED frame, µT. 51 | 52 | - accel_covariance: covariance of accelerometer readings (XYZ) 53 | - gyro_covariance: covariance of gyro readings (XYZ) 54 | - mag_covariance: covariance of magnetometer readings (XYZ) 55 | - gps_position_covariance: covariance of GPS position readings (LLH) 56 | - gps_velocity_covariance: covariance of GPS velocity readings (NED) 57 | - pitot_covariance: covariance of pitot sensor readings 58 | - barometer_amsl_covariance: covariance of barometric pressure sensor 59 | readings 60 | */ 61 | struct ukf_ioboard_params_t { 62 | real_t accel_orientation[4]; /* x, y, z, W */ 63 | real_t accel_offset[3]; /* forwards, starboard, down from CoG (m) */ 64 | real_t gyro_orientation[4]; /* x, y, z, W */ 65 | real_t mag_orientation[4]; /* x, y, z, W */ 66 | real_t mag_field[3]; /* North, East, Down (µT) */ 67 | 68 | real_t accel_covariance[3]; 69 | real_t gyro_covariance[3]; 70 | real_t mag_covariance[3]; 71 | real_t gps_position_covariance[3]; 72 | real_t gps_velocity_covariance[3]; 73 | real_t pitot_covariance; 74 | real_t barometer_amsl_covariance; 75 | }; 76 | 77 | struct ukf_state_t { 78 | real_t position[3]; /* lat (rad), lon (rad), alt (m above ellipsoid) */ 79 | real_t velocity[3]; /* North (m/s), East (m/s), Down (m/s) */ 80 | real_t acceleration[3]; /* forwards (m/s), starboard (m/s), down (m/s) */ 81 | real_t attitude[4]; /* x, y, z, W */ 82 | real_t angular_velocity[3]; /* rolling (rad/s), pitching (rad/s), 83 | yawing (rad/s) */ 84 | real_t angular_acceleration[3]; 85 | real_t wind_velocity[3]; /* North (m/s), East (m/s), Down (m/s) */ 86 | real_t gyro_bias[3]; /* X (rad/s), Y (rad/s), Z (rad/s) */ 87 | }; 88 | 89 | /* 90 | Dynamics model function, for custom model support. These functions have to 91 | be compatible between the C++ and C versions of the UKF, so they take pointers 92 | to C arrays representing the state vector, the control vector, and the output 93 | vector. 94 | */ 95 | typedef void (*ukf_model_function_t)(const real_t *, const real_t *, 96 | real_t *); 97 | 98 | 99 | void ukf_init(void); 100 | 101 | /* Functions for setting different parts of the state vector. */ 102 | void ukf_set_position(real_t lat, real_t lon, real_t alt); 103 | void ukf_set_velocity(real_t x, real_t y, real_t z); 104 | void ukf_set_acceleration(real_t x, real_t y, real_t z); 105 | /* 106 | Note: W, x, y, z in the parameters for ukf_set_attitude differs to the stored 107 | attitude representation in struct ukf_state_t, which is x, y, z, W 108 | */ 109 | void ukf_set_attitude(real_t w, real_t x, real_t y, real_t z); 110 | void ukf_set_angular_velocity(real_t x, real_t y, real_t z); 111 | void ukf_set_angular_acceleration(real_t x, real_t y, real_t z); 112 | void ukf_set_wind_velocity(real_t x, real_t y, real_t z); 113 | void ukf_set_gyro_bias(real_t x, real_t y, real_t z); 114 | 115 | /* Functions for getting the state vector and covariance. */ 116 | void ukf_set_state(struct ukf_state_t *in); 117 | void ukf_get_state(struct ukf_state_t *in); 118 | void ukf_get_state_covariance( 119 | real_t state_covariance[UKF_STATE_DIM * UKF_STATE_DIM]); 120 | void ukf_get_state_covariance_diagonal( 121 | real_t state_covariance_diagonal[UKF_STATE_DIM]); 122 | void ukf_get_state_error(real_t state_error[UKF_STATE_DIM]); 123 | 124 | /* 125 | Functions for setting sensor data. Before each frame, call the sensor_clear() 126 | function to clear old sensor data. 127 | */ 128 | void ukf_sensor_clear(void); 129 | void ukf_sensor_set_accelerometer(real_t x, real_t y, real_t z); 130 | void ukf_sensor_set_gyroscope(real_t x, real_t y, real_t z); 131 | void ukf_sensor_set_magnetometer(real_t x, real_t y, real_t z); 132 | void ukf_sensor_set_gps_position(real_t lat, real_t lon, real_t alt); 133 | void ukf_sensor_set_gps_velocity(real_t x, real_t y, real_t z); 134 | void ukf_sensor_set_pitot_tas(real_t tas); 135 | void ukf_sensor_set_barometer_amsl(real_t amsl); 136 | 137 | /* 138 | UKF-related functions. 139 | */ 140 | void ukf_set_params(struct ukf_ioboard_params_t *in); 141 | void ukf_set_process_noise(real_t process_noise_covariance[UKF_STATE_DIM]); 142 | void ukf_choose_dynamics(enum ukf_model_t t); 143 | void ukf_set_custom_dynamics_model(ukf_model_function_t func); 144 | /* dt is the time delta in seconds */ 145 | void ukf_iterate(float dt, real_t control_vector[UKF_CONTROL_DIM]); 146 | 147 | /* 148 | Functions to access the compiled configuration 149 | */ 150 | enum ukf_precision_t { 151 | UKF_PRECISION_FLOAT = 0, 152 | UKF_PRECISION_DOUBLE = 1 153 | }; 154 | 155 | uint32_t ukf_config_get_state_dim(void); 156 | uint32_t ukf_config_get_measurement_dim(void); 157 | uint32_t ukf_config_get_control_dim(void); 158 | enum ukf_precision_t ukf_config_get_precision(void); 159 | 160 | #ifdef __cplusplus 161 | } 162 | #endif 163 | 164 | #endif 165 | -------------------------------------------------------------------------------- /examples/sfwa_ukf/python/ukf/__init__.py: -------------------------------------------------------------------------------- 1 | #Copyright (C) 2013 Daniel Dyer 2 | # 3 | #Permission is hereby granted, free of charge, to any person obtaining a copy 4 | #of this software and associated documentation files (the "Software"), to deal 5 | #in the Software without restriction, including without limitation the rights 6 | #to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | #copies of the Software, and to permit persons to whom the Software is 8 | #furnished to do so, subject to the following conditions: 9 | # 10 | #The above copyright notice and this permission notice shall be included in 11 | #all copies or substantial portions of the Software. 12 | # 13 | #THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | #IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | #FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | #AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | #LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | #OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | #SOFTWARE. 20 | 21 | import os 22 | from ctypes import * 23 | 24 | 25 | # Taken from c/cukf.h 26 | UKF_PRECISION_FLOAT = 0 27 | UKF_PRECISION_DOUBLE = 1 28 | UKF_MODEL_NONE = 0 29 | UKF_MODEL_CENTRIPETAL = 1 30 | UKF_MODEL_CUSTOM = 2 31 | UKF_MODEL_X8 = 3 32 | 33 | 34 | state = None 35 | covariance = None 36 | model = UKF_MODEL_NONE 37 | custom_model = None 38 | 39 | 40 | # Internal globals, set during init 41 | _cukf = None 42 | _REAL_T = None 43 | _CONTROL_DIM = None 44 | _CUSTOM_MODEL_FUNC = None 45 | 46 | 47 | class _SigmaPoint(object): 48 | def __init__(self, arr): 49 | self.position = (arr[0], arr[1], arr[2]) 50 | self.velocity = (arr[3], arr[4], arr[5]) 51 | self.acceleration = (arr[6], arr[7], arr[8]) 52 | self.attitude = (arr[9], arr[10], arr[11], arr[12]) 53 | self.angular_velocity = (arr[13], arr[14], arr[15]) 54 | self.angular_acceleration = (arr[16], arr[17], arr[18]) 55 | self.wind_velocity = (arr[19], arr[20], arr[21]) 56 | self.gyro_bias = (arr[22], arr[23], arr[24]) 57 | 58 | def __repr__(self): 59 | return str(self.__dict__) 60 | 61 | 62 | # Wrapper around the custom model function 63 | _custom_model_func_wrapper_cb = None # avoid garbage-collection 64 | def _custom_model_wrapper(state, control, output): 65 | if not custom_model: 66 | return 67 | 68 | result = custom_model(_SigmaPoint(state.contents), 69 | tuple(control.contents[0:3])) 70 | 71 | output.contents = (_REAL_T * 6)(*result[0:6]) 72 | 73 | 74 | # Internal classes, wrapping cukf structs directly 75 | class _IOBoardParams(Structure): 76 | pass 77 | 78 | 79 | class _State(Structure): 80 | def __repr__(self): 81 | fields = { 82 | "position": tuple(self.position), 83 | "velocity": tuple(self.velocity), 84 | "acceleration": tuple(self.acceleration), 85 | "attitude": tuple(self.attitude), 86 | "angular_velocity": tuple(self.angular_velocity), 87 | "angular_acceleration": tuple(self.angular_acceleration), 88 | "wind_velocity": tuple(self.wind_velocity), 89 | "gyro_bias": tuple(self.gyro_bias) 90 | } 91 | return str(fields) 92 | 93 | 94 | # Public interface 95 | def iterate(dt, control=None): 96 | global _cukf, state, model 97 | 98 | if not _cukf: 99 | raise RuntimeError("Please call ukf.init()") 100 | 101 | if control is None: 102 | control = (0.0, ) * _CONTROL_DIM 103 | elif len(control) != _CONTROL_DIM: 104 | raise ValueError("Control vector must contain %d elements" % 105 | _CONTROL_DIM) 106 | 107 | _cukf.ukf_choose_dynamics(model) 108 | if model == UKF_MODEL_CUSTOM: 109 | if not custom_model: 110 | raise RuntimeError( 111 | "Can't use ukf.model == UKF_MODEL_CUSTOM without a value " + 112 | "for ukf.custom_model" 113 | ) 114 | 115 | _cukf.ukf_set_state(state) 116 | _cukf.ukf_iterate(dt, (_REAL_T * _CONTROL_DIM)(*control)) 117 | _cukf.ukf_sensor_clear() 118 | _cukf.ukf_get_state(state) 119 | #_cukf.ukf_get_state_covariance(covariance) 120 | 121 | 122 | def set_sensors(accelerometer=None, gyroscope=None, magnetometer=None, 123 | gps_position=None, gps_velocity=None, pitot_tas=None, 124 | barometer_amsl=None): 125 | if accelerometer is not None: 126 | _cukf.ukf_sensor_set_accelerometer(*accelerometer) 127 | if gyroscope is not None: 128 | _cukf.ukf_sensor_set_gyroscope(*gyroscope) 129 | if magnetometer is not None: 130 | _cukf.ukf_sensor_set_magnetometer(*magnetometer) 131 | if gps_position is not None: 132 | _cukf.ukf_sensor_set_gps_position(*gps_position) 133 | if gps_velocity is not None: 134 | _cukf.ukf_sensor_set_gps_velocity(*gps_velocity) 135 | if pitot_tas is not None: 136 | _cukf.ukf_sensor_set_pitot_tas(pitot_tas) 137 | if barometer_amsl is not None: 138 | _cukf.ukf_sensor_set_barometer_amsl(barometer_amsl) 139 | 140 | 141 | def configure_sensors(accelerometer_offset=None, 142 | accelerometer_orientation=None, gyroscope_orientation=None, 143 | magnetometer_orientation=None, wmm_field=None, 144 | accelerometer_covariance=None, gyroscope_covariance=None, 145 | magnetometer_covariance=None, gps_position_covariance=None, 146 | gps_velocity_covariance=None, pitot_tas_covariance=None, 147 | barometer_amsl_covariance=None): 148 | params = _IOBoardParams() 149 | 150 | if accelerometer_offset is not None: 151 | params.accel_offset = accelerometer_offset 152 | else: 153 | params.accel_offset = (0.0, 0.0, 0.0) 154 | 155 | if accelerometer_orientation is not None: 156 | params.accel_orientation = accelerometer_orientation 157 | else: 158 | params.accel_orientation = (0.0, 0.0, 0.0, 1.0) 159 | 160 | if gyroscope_orientation is not None: 161 | params.gyro_orientation = gyroscope_orientation 162 | else: 163 | params.gyro_orientation = (0.0, 0.0, 0.0, 1.0) 164 | 165 | if magnetometer_orientation is not None: 166 | params.mag_orientation = magnetometer_orientation 167 | else: 168 | params.mag_orientation = (0.0, 0.0, 0.0, 1.0) 169 | 170 | if wmm_field is not None: 171 | params.mag_field = wmm_field 172 | else: 173 | params.mag_field = (1.0, 0.0, 0.0) 174 | 175 | if getattr(accelerometer_covariance, '__iter__', False): 176 | params.accel_covariance = accelerometer_covariance 177 | elif accelerometer_covariance is not None: 178 | params.accel_covariance = (accelerometer_covariance, ) * 3 179 | else: 180 | params.accel_covariance = (1.0, 1.0, 1.0) 181 | 182 | if getattr(gyroscope_covariance, '__iter__', False): 183 | params.gyro_covariance = gyroscope_covariance 184 | elif gyroscope_covariance is not None: 185 | params.gyro_covariance = (gyroscope_covariance, ) * 3 186 | else: 187 | params.gyro_covariance = (1.0, 1.0, 1.0) 188 | 189 | if getattr(magnetometer_covariance, '__iter__', False): 190 | params.mag_covariance = magnetometer_covariance 191 | elif magnetometer_covariance is not None: 192 | params.mag_covariance = (magnetometer_covariance, ) * 3 193 | else: 194 | params.mag_covariance = (1.0, 1.0, 1.0) 195 | 196 | if getattr(gps_position_covariance, '__iter__', False): 197 | params.gps_position_covariance = gps_position_covariance 198 | elif gps_position_covariance is not None: 199 | params.gps_position_covariance = (gps_position_covariance, ) * 3 200 | else: 201 | params.gps_position_covariance = (1.0, 1.0, 1.0) 202 | 203 | if getattr(gps_velocity_covariance, '__iter__', False): 204 | params.gps_velocity_covariance = gps_velocity_covariance 205 | elif gps_velocity_covariance is not None: 206 | params.gps_velocity_covariance = (gps_velocity_covariance, ) * 3 207 | else: 208 | params.gps_velocity_covariance = (1.0, 1.0, 1.0) 209 | 210 | if pitot_tas_covariance is not None: 211 | params.pitot_covariance = pitot_tas_covariance 212 | else: 213 | params.pitot_covariance = 1.0 214 | 215 | if barometer_amsl_covariance is not None: 216 | params.barometer_amsl_covariance = barometer_amsl_covariance 217 | else: 218 | params.barometer_amsl_covariance = 1.0 219 | 220 | _cukf.ukf_set_params(params) 221 | 222 | 223 | def configure_process_noise(process_noise_covariance): 224 | _cukf.ukf_set_process_noise((_REAL_T * 24)(*process_noise_covariance)) 225 | 226 | 227 | def init(implementation="c"): 228 | global _cukf, _REAL_T, _CONTROL_DIM, _CUSTOM_MODEL_FUNC, state, \ 229 | _custom_model_func_wrapper_cb 230 | 231 | 232 | # Load the requested library and determine configuration parameters 233 | if implementation == "c": 234 | lib = os.path.join(os.path.dirname(__file__), "c", "libcukf.dylib") 235 | elif implementation == "c66x": 236 | lib = os.path.join(os.path.dirname(__file__), "ccs-c66x", 237 | "libc66ukf.dylib") 238 | else: 239 | raise NameError( 240 | "Unknown UKF implementation: %s (options are 'c', 'c66x')" % 241 | implementation) 242 | 243 | _cukf = cdll.LoadLibrary(lib) 244 | 245 | _cukf.ukf_init.argtypes = [] 246 | _cukf.ukf_init.restype = None 247 | 248 | _cukf.ukf_config_get_precision.argtypes = [] 249 | _cukf.ukf_config_get_precision.restype = c_long 250 | 251 | _cukf.ukf_config_get_state_dim.argtypes = [] 252 | _cukf.ukf_config_get_state_dim.restype = c_long 253 | 254 | _cukf.ukf_config_get_control_dim.argtypes = [] 255 | _cukf.ukf_config_get_control_dim.restype = c_long 256 | 257 | _cukf.ukf_config_get_measurement_dim.argtypes = [] 258 | _cukf.ukf_config_get_measurement_dim.restype = c_long 259 | 260 | _PRECISION = _cukf.ukf_config_get_precision() 261 | _REAL_T = c_double if _PRECISION == UKF_PRECISION_DOUBLE else c_float 262 | _CONTROL_DIM = _cukf.ukf_config_get_control_dim() 263 | _STATE_DIM = _cukf.ukf_config_get_state_dim() 264 | _MEASUREMENT_DIM = _cukf.ukf_config_get_measurement_dim() 265 | 266 | _IOBoardParams._fields_ = [ 267 | ("accel_orientation", _REAL_T * 4), 268 | ("accel_offset", _REAL_T * 3), 269 | ("gyro_orientation", _REAL_T * 4), 270 | ("mag_orientation", _REAL_T * 4), 271 | ("mag_field", _REAL_T * 3), 272 | 273 | ("accel_covariance", _REAL_T * 3), 274 | ("gyro_covariance", _REAL_T * 3), 275 | ("mag_covariance", _REAL_T * 3), 276 | ("gps_position_covariance", _REAL_T * 3), 277 | ("gps_velocity_covariance", _REAL_T * 3), 278 | ("pitot_covariance", _REAL_T), 279 | ("barometer_amsl_covariance", _REAL_T) 280 | ] 281 | 282 | _State._fields_ = [ 283 | ("position", _REAL_T * 3), 284 | ("velocity", _REAL_T * 3), 285 | ("acceleration", _REAL_T * 3), 286 | ("attitude", _REAL_T * 4), 287 | ("angular_velocity", _REAL_T * 3), 288 | ("angular_acceleration", _REAL_T * 3), 289 | ("wind_velocity", _REAL_T * 3), 290 | ("gyro_bias", _REAL_T * 3) 291 | ] 292 | 293 | # Set up the function prototypes 294 | _cukf.ukf_set_position.argtypes = [_REAL_T, _REAL_T, _REAL_T] 295 | _cukf.ukf_set_position.restype = None 296 | 297 | _cukf.ukf_set_velocity.argtypes = [_REAL_T, _REAL_T, _REAL_T] 298 | _cukf.ukf_set_velocity.restype = None 299 | 300 | _cukf.ukf_set_acceleration.argtypes = [_REAL_T, _REAL_T, _REAL_T] 301 | _cukf.ukf_set_acceleration.restype = None 302 | 303 | _cukf.ukf_set_attitude.argtypes = [_REAL_T, _REAL_T, _REAL_T, _REAL_T] 304 | _cukf.ukf_set_attitude.restype = None 305 | 306 | _cukf.ukf_set_angular_velocity.argtypes = [_REAL_T, _REAL_T, _REAL_T] 307 | _cukf.ukf_set_angular_velocity.restype = None 308 | 309 | _cukf.ukf_set_angular_acceleration.argtypes = [_REAL_T, _REAL_T, _REAL_T] 310 | _cukf.ukf_set_angular_acceleration.restype = None 311 | 312 | _cukf.ukf_set_wind_velocity.argtypes = [_REAL_T, _REAL_T, _REAL_T] 313 | _cukf.ukf_set_wind_velocity.restype = None 314 | 315 | _cukf.ukf_set_gyro_bias.argtypes = [_REAL_T, _REAL_T, _REAL_T] 316 | _cukf.ukf_set_gyro_bias.restype = None 317 | 318 | _cukf.ukf_get_state.argtypes = [POINTER(_State)] 319 | _cukf.ukf_get_state.restype = None 320 | 321 | _cukf.ukf_set_state.argtypes = [POINTER(_State)] 322 | _cukf.ukf_set_state.restype = None 323 | 324 | _cukf.ukf_get_state_covariance.argtypes = [ 325 | POINTER(_REAL_T * (_STATE_DIM**2))] 326 | _cukf.ukf_get_state_covariance.restype = None 327 | 328 | _cukf.ukf_sensor_clear.argtypes = [] 329 | _cukf.ukf_sensor_clear.restype = None 330 | 331 | _cukf.ukf_sensor_set_accelerometer.argtypes = [_REAL_T, _REAL_T, _REAL_T] 332 | _cukf.ukf_sensor_set_accelerometer.restype = None 333 | 334 | _cukf.ukf_sensor_set_gyroscope.argtypes = [_REAL_T, _REAL_T, _REAL_T] 335 | _cukf.ukf_sensor_set_gyroscope.restype = None 336 | 337 | _cukf.ukf_sensor_set_magnetometer.argtypes = [_REAL_T, _REAL_T, _REAL_T] 338 | _cukf.ukf_sensor_set_magnetometer.restype = None 339 | 340 | _cukf.ukf_sensor_set_gps_position.argtypes = [_REAL_T, _REAL_T, _REAL_T] 341 | _cukf.ukf_sensor_set_gps_position.restype = None 342 | 343 | _cukf.ukf_sensor_set_gps_velocity.argtypes = [_REAL_T, _REAL_T, _REAL_T] 344 | _cukf.ukf_sensor_set_gps_velocity.restype = None 345 | 346 | _cukf.ukf_sensor_set_pitot_tas.argtypes = [_REAL_T] 347 | _cukf.ukf_sensor_set_pitot_tas.restype = None 348 | 349 | _cukf.ukf_sensor_set_barometer_amsl.argtypes = [_REAL_T] 350 | _cukf.ukf_sensor_set_barometer_amsl.restype = None 351 | 352 | _cukf.ukf_set_params.argtypes = [POINTER(_IOBoardParams)] 353 | _cukf.ukf_set_params.restype = None 354 | 355 | _cukf.ukf_choose_dynamics.argtypes = [c_int] 356 | _cukf.ukf_choose_dynamics.restype = None 357 | 358 | _CUSTOM_MODEL_FUNC = CFUNCTYPE(None, 359 | POINTER(_REAL_T * (_STATE_DIM + 1)), 360 | POINTER(_REAL_T * _CONTROL_DIM), 361 | POINTER(_REAL_T * 6)) 362 | 363 | _cukf.ukf_set_custom_dynamics_model.argtypes = [_CUSTOM_MODEL_FUNC] 364 | _cukf.ukf_set_custom_dynamics_model.restype = None 365 | 366 | _cukf.ukf_iterate.argtypes = [c_float, POINTER(_REAL_T * _CONTROL_DIM)] 367 | _cukf.ukf_iterate.restype = None 368 | 369 | _cukf.ukf_set_process_noise.argtypes = [POINTER(_REAL_T * _STATE_DIM)] 370 | _cukf.ukf_set_process_noise.restype = None 371 | 372 | # Initialize the library 373 | _cukf.ukf_init() 374 | 375 | # Set the custom model callback 376 | _custom_model_func_wrapper_cb = _CUSTOM_MODEL_FUNC(_custom_model_wrapper) 377 | _cukf.ukf_set_custom_dynamics_model(_custom_model_func_wrapper_cb) 378 | 379 | # Set up the state 380 | state = _State() 381 | _cukf.ukf_get_state(state) 382 | -------------------------------------------------------------------------------- /include/UKF/Config.h: -------------------------------------------------------------------------------- 1 | /* Selects the floating-point precision to use. */ 2 | #if defined(UKF_DOUBLE_PRECISION) 3 | typedef double real_t; 4 | #elif defined(UKF_SINGLE_PRECISION) 5 | typedef float real_t; 6 | #else 7 | #error "Define the floating-point precision using either UKF_DOUBLE_PRECISION or UKF_SINGLE_PRECISION" 8 | #endif -------------------------------------------------------------------------------- /include/UKF/Integrator.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2016 Thiemar Pty Ltd 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #ifndef INTEGRATOR_H_ 24 | #define INTEGRATOR_H_ 25 | 26 | #include "UKF/Config.h" 27 | 28 | namespace UKF { 29 | 30 | /* Fourth-order integrator. */ 31 | class IntegratorRK4 { 32 | public: 33 | template 34 | static S integrate(real_t delta, const S& state, const U&... input) { 35 | S a = state.derivative(input...); 36 | S b = S(state + real_t(0.5) * delta * a).derivative(input...); 37 | S c = S(state + real_t(0.5) * delta * b).derivative(input...); 38 | S d = S(state + delta * c).derivative(input...); 39 | return state + (delta / real_t(6.0)) * (a + (b * real_t(2.0)) + (c * real_t(2.0)) + d); 40 | } 41 | }; 42 | 43 | /* Second-order integrator. */ 44 | class IntegratorHeun { 45 | public: 46 | template 47 | static S integrate(real_t delta, const S& state, const U&... input) { 48 | S initial = state.derivative(input...); 49 | S predictor = state + delta * initial; 50 | return state + (delta * real_t(0.5)) * (initial + predictor.derivative(input...)); 51 | } 52 | }; 53 | 54 | /* First-order integrator. */ 55 | class IntegratorEuler { 56 | public: 57 | template 58 | static S integrate(real_t delta, const S& state, const U&... input) { 59 | return state + delta * state.derivative(input...); 60 | } 61 | }; 62 | 63 | } 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /include/UKF/Types.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2016 Thiemar Pty Ltd 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #ifndef TYPES_H 24 | #define TYPES_H 25 | 26 | #include 27 | #include "UKF/Config.h" 28 | 29 | namespace UKF { 30 | 31 | template 32 | using Matrix = Eigen::Matrix; 33 | 34 | template 35 | using MatrixDynamic = Eigen::Matrix; 36 | 37 | template 38 | using Array = Eigen::Array; 39 | 40 | template 41 | using Vector = Eigen::Matrix; 42 | 43 | template 44 | using VectorDynamic = Eigen::Matrix; 45 | 46 | using Quaternion = Eigen::Quaternion; 47 | 48 | class FieldVector : public Vector<3> { 49 | public: 50 | /* Inherit Eigen::Matrix constructors and assignment operators. */ 51 | using Base = Vector<3>; 52 | using Base::Base; 53 | using Base::operator=; 54 | }; 55 | 56 | } 57 | 58 | #endif -------------------------------------------------------------------------------- /patch/eigen-3.2.8-no-malloc-in-llt.patch: -------------------------------------------------------------------------------- 1 | diff --git a/Eigen/src/Cholesky/LLT.h b/Eigen/src/Cholesky/LLT.h 2 | index 7c11a2d..21b824d 100644 3 | --- a/Eigen/src/Cholesky/LLT.h 4 | +++ b/Eigen/src/Cholesky/LLT.h 5 | @@ -203,7 +203,7 @@ static typename MatrixType::Index llt_rank_update_lower(MatrixType& mat, const V 6 | typedef typename MatrixType::ColXpr ColXpr; 7 | typedef typename internal::remove_all::type ColXprCleaned; 8 | typedef typename ColXprCleaned::SegmentReturnType ColXprSegment; 9 | - typedef Matrix TempVectorType; 10 | + typedef Matrix TempVectorType; 11 | typedef typename TempVectorType::SegmentReturnType TempVecSegment; 12 | 13 | Index n = mat.cols(); 14 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8.4 FATAL_ERROR) 2 | PROJECT(unittest) 3 | INCLUDE(ExternalProject) 4 | 5 | # Set default ExternalProject root directory 6 | SET_DIRECTORY_PROPERTIES(PROPERTIES EP_PREFIX .) 7 | 8 | # Add gtest 9 | ExternalProject_Add( 10 | googletest 11 | URL https://github.com/google/googletest/archive/release-1.7.0.zip 12 | TIMEOUT 30 13 | CMAKE_ARGS -DCMAKE_BUILD_TYPE:STRING=Release 14 | # Disable install step 15 | INSTALL_COMMAND "" 16 | # Wrap download, configure and build steps in a script to log output 17 | LOG_DOWNLOAD ON 18 | LOG_CONFIGURE ON 19 | LOG_BUILD ON) 20 | 21 | # Disable dynamic memory allocation in Eigen 22 | ADD_DEFINITIONS(-DEIGEN_NO_MALLOC -DUKF_DOUBLE_PRECISION) 23 | 24 | # Specify include dir 25 | ExternalProject_Get_Property(googletest source_dir) 26 | SET(googletest_dir ${source_dir}) 27 | 28 | INCLUDE_DIRECTORIES(${googletest_dir}/include ../include ${eigen_dir}) 29 | 30 | # Add test executable target 31 | ADD_EXECUTABLE(unittest 32 | TestStateVector.cpp 33 | TestFixedMeasurementVector.cpp 34 | TestDynamicMeasurementVector.cpp 35 | TestCore.cpp 36 | TestSquareRootCore.cpp) 37 | 38 | # Create dependency of test on googletest 39 | ADD_DEPENDENCIES(unittest googletest eigen3) 40 | 41 | # Specify test's link libraries 42 | ExternalProject_Get_Property(googletest binary_dir) 43 | TARGET_LINK_LIBRARIES(unittest 44 | ${binary_dir}/${CMAKE_FIND_LIBRARY_PREFIXES}gtest.a 45 | ${binary_dir}/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main.a 46 | pthread) 47 | 48 | ADD_TEST(unittest unittest) 49 | ADD_CUSTOM_TARGET(check COMMAND ${CMAKE_CTEST_COMMAND} DEPENDS unittest) 50 | -------------------------------------------------------------------------------- /test/TestCore.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "UKF/Types.h" 6 | #include "UKF/Integrator.h" 7 | #include "UKF/StateVector.h" 8 | #include "UKF/MeasurementVector.h" 9 | #include "UKF/Core.h" 10 | #include "comparisons.h" 11 | 12 | /* Set up state vector class. */ 13 | enum MyStateFields { 14 | Position, 15 | Velocity, 16 | Attitude, 17 | AngularVelocity 18 | }; 19 | 20 | using MyStateVector = UKF::StateVector< 21 | UKF::Field>, 22 | UKF::Field>, 23 | UKF::Field, 24 | UKF::Field> 25 | >; 26 | namespace UKF { 27 | namespace Parameters { 28 | template <> constexpr real_t AlphaSquared = 1e-6; 29 | } 30 | 31 | /* 32 | State vector process model. One version takes body frame kinematic 33 | acceleration and angular acceleration as inputs, the other doesn't (assumes 34 | zero accelerations). 35 | */ 36 | template <> template <> 37 | MyStateVector MyStateVector::derivative, UKF::Vector<3>>( 38 | const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) const { 39 | MyStateVector temp; 40 | 41 | /* Position derivative. */ 42 | temp.set_field(get_field()); 43 | 44 | /* Velocity derivative. */ 45 | temp.set_field(get_field().conjugate() * acceleration); 46 | 47 | /* Attitude derivative. */ 48 | UKF::Quaternion temp_q; 49 | temp_q.vec() = get_field(); 50 | temp_q.w() = 0; 51 | temp.set_field(temp_q); 52 | 53 | /* Angular velocity derivative. */ 54 | temp.set_field(angular_acceleration); 55 | 56 | return temp; 57 | } 58 | 59 | template <> template <> 60 | MyStateVector MyStateVector::derivative<>() const { 61 | return derivative(UKF::Vector<3>(0, 0, 0), UKF::Vector<3>(0, 0, 0)); 62 | } 63 | 64 | } 65 | 66 | /* Set up measurement vector class. */ 67 | enum MyMeasurementFields { 68 | GPS_Position, 69 | GPS_Velocity, 70 | Accelerometer, 71 | Magnetometer, 72 | Gyroscope 73 | }; 74 | 75 | using MyMeasurementVector = UKF::DynamicMeasurementVector< 76 | UKF::Field>, 77 | UKF::Field>, 78 | UKF::Field>, 79 | UKF::Field, 80 | UKF::Field> 81 | >; 82 | 83 | using MyCore = UKF::Core< 84 | MyStateVector, 85 | MyMeasurementVector, 86 | UKF::IntegratorRK4 87 | >; 88 | 89 | namespace UKF { 90 | /* 91 | Define measurement model to be used in tests. NOTE: These are just for 92 | testing, don't expect them to make any physical sense whatsoever. 93 | */ 94 | template <> template <> 95 | UKF::Vector<3> MyMeasurementVector::expected_measurement 96 | (const MyStateVector& state) { 97 | return state.get_field(); 98 | } 99 | 100 | template <> template <> 101 | UKF::Vector<3> MyMeasurementVector::expected_measurement 102 | (const MyStateVector& state) { 103 | return state.get_field(); 104 | } 105 | 106 | template <> template <> 107 | UKF::Vector<3> MyMeasurementVector::expected_measurement 108 | (const MyStateVector& state) { 109 | return state.get_field() * UKF::Vector<3>(0, 0, -9.8); 110 | } 111 | 112 | template <> template <> 113 | UKF::FieldVector MyMeasurementVector::expected_measurement 114 | (const MyStateVector& state) { 115 | return state.get_field() * UKF::FieldVector(1, 0, 0); 116 | } 117 | 118 | template <> template <> 119 | UKF::Vector<3> MyMeasurementVector::expected_measurement 120 | (const MyStateVector& state) { 121 | return state.get_field(); 122 | } 123 | 124 | /* 125 | These versions of the predicted measurement functions take kinematic 126 | acceleration and angular acceleration as inputs. Note that in reality, the 127 | inputs would probably be a control vector and the accelerations would be 128 | calculated using the state vector and a dynamics model. 129 | */ 130 | template <> template <> 131 | UKF::Vector<3> MyMeasurementVector::expected_measurement 132 | (const MyStateVector& state, 133 | const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) { 134 | return state.get_field(); 135 | } 136 | 137 | template <> template <> 138 | UKF::Vector<3> MyMeasurementVector::expected_measurement 139 | (const MyStateVector& state, 140 | const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) { 141 | return state.get_field(); 142 | } 143 | 144 | template <> template <> 145 | UKF::Vector<3> MyMeasurementVector::expected_measurement 146 | >(const MyStateVector& state, 147 | const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) { 148 | return state.get_field() * UKF::Vector<3>(0, 0, -9.8) + acceleration; 149 | } 150 | 151 | template <> template <> 152 | UKF::FieldVector MyMeasurementVector::expected_measurement 153 | >(const MyStateVector& state, 154 | const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) { 155 | return state.get_field() * UKF::FieldVector(1, 0, 0); 156 | } 157 | 158 | template <> template <> 159 | UKF::Vector<3> MyMeasurementVector::expected_measurement 160 | >(const MyStateVector& state, 161 | const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) { 162 | return state.get_field(); 163 | } 164 | 165 | } 166 | 167 | MyCore create_initialised_test_filter() { 168 | MyCore test_filter; 169 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 170 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 171 | test_filter.state.set_field(UKF::Quaternion(1, 0, 0, 0)); 172 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 173 | test_filter.covariance = MyStateVector::CovarianceMatrix::Zero(); 174 | test_filter.covariance.diagonal() << 10000, 10000, 10000, 100, 100, 100, 1, 1, 5, 10, 10, 10; 175 | test_filter.measurement_covariance << 10, 10, 10, 1, 1, 1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 0.05, 0.05, 0.05; 176 | 177 | real_t a, b; 178 | real_t dt = 0.01; 179 | a = std::sqrt(0.1*dt*dt); 180 | b = std::sqrt(0.1*dt); 181 | test_filter.process_noise_covariance << a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 182 | 0, a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 183 | 0, 0, a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 184 | 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 0, 0, 185 | 0, 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 0, 186 | 0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 187 | 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 0, 188 | 0, 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 189 | 0, 0, 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 190 | 0, 0, 0, 0, 0, 0, 0, 0, 0, b, 0, 0, 191 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, b, 0, 192 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, b; 193 | 194 | return test_filter; 195 | } 196 | 197 | TEST(CoreTest, Initialisation) { 198 | MyCore test_filter = create_initialised_test_filter(); 199 | } 200 | 201 | /* 202 | All these tests check that the estimated state matches the 'actual' state to 203 | within 2-sigma. 204 | */ 205 | TEST(CoreTest, APrioriStep) { 206 | MyCore test_filter = create_initialised_test_filter(); 207 | 208 | test_filter.a_priori_step(0.01); 209 | 210 | EXPECT_GT(test_filter.covariance.determinant(), std::numeric_limits::epsilon()); 211 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 212 | std::sqrt(test_filter.covariance.diagonal().segment<3>(0).norm())*2); 213 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 214 | std::sqrt(test_filter.covariance.diagonal().segment<3>(3).norm())*2); 215 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 216 | std::sqrt(test_filter.covariance.diagonal().segment<3>(6).norm())*2); 217 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 218 | std::sqrt(test_filter.covariance.diagonal().segment<3>(9).norm())*2); 219 | } 220 | 221 | TEST(CoreTest, APrioriStepWithInputs) { 222 | MyCore test_filter = create_initialised_test_filter(); 223 | 224 | test_filter.a_priori_step(0.01, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0)); 225 | 226 | EXPECT_GT(test_filter.covariance.determinant(), std::numeric_limits::epsilon()); 227 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 228 | std::sqrt(test_filter.covariance.diagonal().segment<3>(0).norm())*2); 229 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 230 | std::sqrt(test_filter.covariance.diagonal().segment<3>(3).norm())*2); 231 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 232 | std::sqrt(test_filter.covariance.diagonal().segment<3>(6).norm())*2); 233 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 234 | std::sqrt(test_filter.covariance.diagonal().segment<3>(9).norm())*2); 235 | } 236 | 237 | TEST(CoreTest, InnovationStep) { 238 | MyCore test_filter = create_initialised_test_filter(); 239 | MyMeasurementVector m; 240 | 241 | m.set_field(UKF::Vector<3>(100, 10, -50)); 242 | m.set_field(UKF::Vector<3>(20, 0, 0)); 243 | m.set_field(UKF::Vector<3>(0, 0, -9.8)); 244 | m.set_field(UKF::FieldVector(0, -1, 0)); 245 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 246 | 247 | test_filter.a_priori_step(0.01); 248 | test_filter.innovation_step(m); 249 | 250 | /* 251 | With the field vector, we expect the determinant to be approximately zero, 252 | so allow for it to be slightly negative due to numerical precision. 253 | */ 254 | EXPECT_GE(test_filter.innovation_covariance.determinant(), -std::numeric_limits::epsilon()); 255 | 256 | test_filter.a_posteriori_step(); 257 | 258 | EXPECT_GT(test_filter.covariance.determinant(), std::numeric_limits::epsilon()); 259 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 260 | std::sqrt(test_filter.covariance.diagonal().segment<3>(0).norm())*2); 261 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 262 | std::sqrt(test_filter.covariance.diagonal().segment<3>(3).norm())*2); 263 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 264 | std::sqrt(test_filter.covariance.diagonal().segment<3>(6).norm())*2); 265 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 266 | std::sqrt(test_filter.covariance.diagonal().segment<3>(9).norm())*2); 267 | } 268 | 269 | TEST(CoreTest, InnovationStepPartialMeasurement) { 270 | MyCore test_filter = create_initialised_test_filter(); 271 | MyMeasurementVector m; 272 | 273 | m.set_field(UKF::Vector<3>(0, 0, -9.8)); 274 | m.set_field(UKF::FieldVector(0, -1, 0)); 275 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 276 | 277 | test_filter.a_priori_step(0.01); 278 | test_filter.innovation_step(m); 279 | 280 | EXPECT_GE(test_filter.innovation_covariance.determinant(), -std::numeric_limits::epsilon()); 281 | 282 | test_filter.a_posteriori_step(); 283 | 284 | EXPECT_GT(test_filter.covariance.determinant(), std::numeric_limits::epsilon()); 285 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 286 | std::sqrt(test_filter.covariance.diagonal().segment<3>(0).norm())*2); 287 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 288 | std::sqrt(test_filter.covariance.diagonal().segment<3>(3).norm())*2); 289 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 290 | std::sqrt(test_filter.covariance.diagonal().segment<3>(6).norm())*2); 291 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 292 | std::sqrt(test_filter.covariance.diagonal().segment<3>(9).norm())*2); 293 | } 294 | 295 | TEST(CoreTest, InnovationStepWithInputs) { 296 | MyCore test_filter = create_initialised_test_filter(); 297 | MyMeasurementVector m; 298 | 299 | m.set_field(UKF::Vector<3>(100, 10, -50)); 300 | m.set_field(UKF::Vector<3>(20, 0, 0)); 301 | m.set_field(UKF::Vector<3>(0, 0, -15)); 302 | m.set_field(UKF::FieldVector(0, -1, 0)); 303 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 304 | 305 | test_filter.a_priori_step(0.01, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0)); 306 | test_filter.innovation_step(m, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0)); 307 | 308 | EXPECT_GE(test_filter.innovation_covariance.determinant(), -std::numeric_limits::epsilon()); 309 | 310 | test_filter.a_posteriori_step(); 311 | 312 | EXPECT_GT(test_filter.covariance.determinant(), std::numeric_limits::epsilon()); 313 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 314 | std::sqrt(test_filter.covariance.diagonal().segment<3>(0).norm())*2); 315 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 316 | std::sqrt(test_filter.covariance.diagonal().segment<3>(3).norm())*2); 317 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 318 | std::sqrt(test_filter.covariance.diagonal().segment<3>(6).norm())*2); 319 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 320 | std::sqrt(test_filter.covariance.diagonal().segment<3>(9).norm())*2); 321 | } 322 | 323 | TEST(CoreTest, InnovationStepPartialMeasurementWithInputs) { 324 | MyCore test_filter = create_initialised_test_filter(); 325 | MyMeasurementVector m; 326 | 327 | m.set_field(UKF::Vector<3>(0, 0, -15)); 328 | m.set_field(UKF::FieldVector(0, -1, 0)); 329 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 330 | 331 | test_filter.a_priori_step(0.01, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0)); 332 | test_filter.innovation_step(m, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0)); 333 | 334 | EXPECT_GE(test_filter.innovation_covariance.determinant(), -std::numeric_limits::epsilon()); 335 | 336 | test_filter.a_posteriori_step(); 337 | 338 | EXPECT_GT(test_filter.covariance.determinant(), std::numeric_limits::epsilon()); 339 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 340 | std::sqrt(test_filter.covariance.diagonal().segment<3>(0).norm())*2); 341 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 342 | std::sqrt(test_filter.covariance.diagonal().segment<3>(3).norm())*2); 343 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 344 | std::sqrt(test_filter.covariance.diagonal().segment<3>(6).norm())*2); 345 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 346 | std::sqrt(test_filter.covariance.diagonal().segment<3>(9).norm())*2); 347 | } 348 | 349 | TEST(CoreTest, APosterioriStep) { 350 | MyCore test_filter = create_initialised_test_filter(); 351 | MyMeasurementVector m; 352 | 353 | m.set_field(UKF::Vector<3>(100, 10, -50)); 354 | m.set_field(UKF::Vector<3>(20, 0, 0)); 355 | m.set_field(UKF::Vector<3>(0, 0, -9.8)); 356 | m.set_field(UKF::FieldVector(0, -1, 0)); 357 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 358 | 359 | test_filter.a_priori_step(0.01); 360 | test_filter.innovation_step(m); 361 | test_filter.a_posteriori_step(); 362 | 363 | EXPECT_GT(test_filter.covariance.determinant(), std::numeric_limits::epsilon()); 364 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 365 | std::sqrt(test_filter.covariance.diagonal().segment<3>(0).norm())*2); 366 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 367 | std::sqrt(test_filter.covariance.diagonal().segment<3>(3).norm())*2); 368 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 369 | std::sqrt(test_filter.covariance.diagonal().segment<3>(6).norm())*2); 370 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 371 | std::sqrt(test_filter.covariance.diagonal().segment<3>(9).norm())*2); 372 | } 373 | 374 | TEST(CoreTest, FullStep) { 375 | MyCore test_filter = create_initialised_test_filter(); 376 | MyMeasurementVector m; 377 | 378 | m.set_field(UKF::Vector<3>(100, 10, -50)); 379 | m.set_field(UKF::Vector<3>(20, 0, 0)); 380 | m.set_field(UKF::Vector<3>(0, 0, -14.8)); 381 | m.set_field(UKF::FieldVector(0, -1, 0)); 382 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 383 | 384 | test_filter.step(0.01, m, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0)); 385 | 386 | EXPECT_GT(test_filter.covariance.determinant(), std::numeric_limits::epsilon()); 387 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 388 | std::sqrt(test_filter.covariance.diagonal().segment<3>(0).norm())*2); 389 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 390 | std::sqrt(test_filter.covariance.diagonal().segment<3>(3).norm())*2); 391 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 392 | std::sqrt(test_filter.covariance.diagonal().segment<3>(6).norm())*2); 393 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 394 | std::sqrt(test_filter.covariance.diagonal().segment<3>(9).norm())*2); 395 | } 396 | -------------------------------------------------------------------------------- /test/TestSquareRootCore.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "UKF/Types.h" 6 | #include "UKF/Integrator.h" 7 | #include "UKF/StateVector.h" 8 | #include "UKF/MeasurementVector.h" 9 | #include "UKF/Core.h" 10 | #include "comparisons.h" 11 | 12 | /* 13 | Set up state vector class. The order of these is changed to prevent 14 | linker collisions with the ones in TestCore.cpp. 15 | */ 16 | enum MyStateFields { 17 | Attitude, 18 | AngularVelocity, 19 | Position, 20 | Velocity 21 | }; 22 | 23 | using MyStateVector = UKF::StateVector< 24 | UKF::Field>, 25 | UKF::Field>, 26 | UKF::Field, 27 | UKF::Field> 28 | >; 29 | 30 | namespace UKF { 31 | namespace Parameters { 32 | 33 | template <> constexpr real_t AlphaSquared = 1e-6; 34 | } 35 | 36 | /* 37 | State vector process model. One version takes body frame kinematic 38 | acceleration and angular acceleration as inputs, the other doesn't (assumes 39 | zero accelerations). 40 | */ 41 | template <> template <> 42 | MyStateVector MyStateVector::derivative, UKF::Vector<3>>( 43 | const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) const { 44 | MyStateVector temp; 45 | 46 | /* Position derivative. */ 47 | temp.set_field(get_field()); 48 | 49 | /* Velocity derivative. */ 50 | temp.set_field(get_field().conjugate() * acceleration); 51 | 52 | /* Attitude derivative. */ 53 | UKF::Quaternion temp_q; 54 | temp_q.vec() = get_field(); 55 | temp_q.w() = 0; 56 | temp.set_field(temp_q); 57 | 58 | /* Angular velocity derivative. */ 59 | temp.set_field(angular_acceleration); 60 | 61 | return temp; 62 | } 63 | 64 | template <> template <> 65 | MyStateVector MyStateVector::derivative<>() const { 66 | return derivative(UKF::Vector<3>(0, 0, 0), UKF::Vector<3>(0, 0, 0)); 67 | } 68 | 69 | } 70 | 71 | /* 72 | Set up measurement vector class. The order of these is changed to prevent 73 | linker collisions with the ones in TestCore.cpp. 74 | */ 75 | enum MyMeasurementFields { 76 | Accelerometer, 77 | Magnetometer, 78 | Gyroscope, 79 | GPS_Position, 80 | GPS_Velocity 81 | }; 82 | 83 | using MyMeasurementVector = UKF::DynamicMeasurementVector< 84 | UKF::Field>, 85 | UKF::Field>, 86 | UKF::Field>, 87 | UKF::Field, 88 | UKF::Field> 89 | >; 90 | 91 | using MyCore = UKF::SquareRootCore< 92 | MyStateVector, 93 | MyMeasurementVector, 94 | UKF::IntegratorRK4 95 | >; 96 | 97 | namespace UKF { 98 | /* 99 | Define measurement model to be used in tests. NOTE: These are just for 100 | testing, don't expect them to make any physical sense whatsoever. 101 | */ 102 | template <> template <> 103 | UKF::Vector<3> MyMeasurementVector::expected_measurement 104 | (const MyStateVector& state) { 105 | return state.get_field(); 106 | } 107 | 108 | template <> template <> 109 | UKF::Vector<3> MyMeasurementVector::expected_measurement 110 | (const MyStateVector& state) { 111 | return state.get_field(); 112 | } 113 | 114 | template <> template <> 115 | UKF::Vector<3> MyMeasurementVector::expected_measurement 116 | (const MyStateVector& state) { 117 | return state.get_field() * UKF::Vector<3>(0, 0, -9.8); 118 | } 119 | 120 | template <> template <> 121 | UKF::FieldVector MyMeasurementVector::expected_measurement 122 | (const MyStateVector& state) { 123 | return state.get_field() * UKF::FieldVector(1, 0, 0); 124 | } 125 | 126 | template <> template <> 127 | UKF::Vector<3> MyMeasurementVector::expected_measurement 128 | (const MyStateVector& state) { 129 | return state.get_field(); 130 | } 131 | 132 | /* 133 | These versions of the predicted measurement functions take kinematic 134 | acceleration and angular acceleration as inputs. Note that in reality, the 135 | inputs would probably be a control vector and the accelerations would be 136 | calculated using the state vector and a dynamics model. 137 | */ 138 | template <> template <> 139 | UKF::Vector<3> MyMeasurementVector::expected_measurement 140 | (const MyStateVector& state, 141 | const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) { 142 | return state.get_field(); 143 | } 144 | 145 | template <> template <> 146 | UKF::Vector<3> MyMeasurementVector::expected_measurement 147 | (const MyStateVector& state, 148 | const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) { 149 | return state.get_field(); 150 | } 151 | 152 | template <> template <> 153 | UKF::Vector<3> MyMeasurementVector::expected_measurement 154 | >(const MyStateVector& state, 155 | const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) { 156 | return state.get_field() * UKF::Vector<3>(0, 0, -9.8) + acceleration; 157 | } 158 | 159 | template <> template <> 160 | UKF::FieldVector MyMeasurementVector::expected_measurement 161 | >(const MyStateVector& state, 162 | const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) { 163 | return state.get_field() * UKF::FieldVector(1, 0, 0); 164 | } 165 | 166 | template <> template <> 167 | UKF::Vector<3> MyMeasurementVector::expected_measurement 168 | >(const MyStateVector& state, 169 | const UKF::Vector<3>& acceleration, const UKF::Vector<3>& angular_acceleration) { 170 | return state.get_field(); 171 | } 172 | 173 | } 174 | 175 | /* 176 | Initialise covariances as square roots to be comparable with TestCore.cpp. 177 | */ 178 | MyCore create_initialised_sr_test_filter() { 179 | MyCore test_filter; 180 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 181 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 182 | test_filter.state.set_field(UKF::Quaternion(1, 0, 0, 0)); 183 | test_filter.state.set_field(UKF::Vector<3>(0, 0, 0)); 184 | test_filter.root_covariance = MyStateVector::CovarianceMatrix::Zero(); 185 | test_filter.root_covariance.diagonal() << 186 | 10000, 10000, 10000, 100, 100, 100, 1, 1, 5, 10, 10, 10; 187 | test_filter.root_covariance = test_filter.root_covariance.llt().matrixU(); 188 | test_filter.measurement_root_covariance << 189 | 10, 10, 10, 1, 1, 1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 5e-1, 0.05, 0.05, 0.05; 190 | test_filter.measurement_root_covariance = test_filter.measurement_root_covariance.array().sqrt(); 191 | 192 | real_t a, b; 193 | real_t dt = 0.01; 194 | a = std::sqrt(0.1*dt*dt); 195 | b = std::sqrt(0.1*dt); 196 | test_filter.process_noise_root_covariance << a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 197 | 0, a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 198 | 0, 0, a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 199 | 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 0, 0, 200 | 0, 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 0, 201 | 0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, 0, 202 | 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 0, 203 | 0, 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 204 | 0, 0, 0, 0, 0, 0, 0, 0, a, 0, 0, 0, 205 | 0, 0, 0, 0, 0, 0, 0, 0, 0, b, 0, 0, 206 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, b, 0, 207 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, b; 208 | test_filter.process_noise_root_covariance = test_filter.process_noise_root_covariance.llt().matrixU(); 209 | 210 | return test_filter; 211 | } 212 | 213 | TEST(SquareRootCoreTest, Initialisation) { 214 | MyCore test_filter = create_initialised_sr_test_filter(); 215 | } 216 | 217 | /* 218 | All these tests check that the estimated state matches the 'actual' state to 219 | within 2-sigma. 220 | */ 221 | TEST(SquareRootCoreTest, APrioriStep) { 222 | MyCore test_filter = create_initialised_sr_test_filter(); 223 | 224 | test_filter.a_priori_step(0.01); 225 | 226 | EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits::epsilon()); 227 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 228 | test_filter.root_covariance.diagonal().segment<3>(0).norm()*2); 229 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 230 | test_filter.root_covariance.diagonal().segment<3>(3).norm()*2); 231 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 232 | test_filter.root_covariance.diagonal().segment<3>(6).norm()*2); 233 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 234 | test_filter.root_covariance.diagonal().segment<3>(9).norm()*2); 235 | } 236 | 237 | TEST(SquareRootCoreTest, APrioriStepWithInputs) { 238 | MyCore test_filter = create_initialised_sr_test_filter(); 239 | 240 | test_filter.a_priori_step(0.01, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0)); 241 | 242 | EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits::epsilon()); 243 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 244 | test_filter.root_covariance.diagonal().segment<3>(0).norm()*2); 245 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 246 | test_filter.root_covariance.diagonal().segment<3>(3).norm()*2); 247 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 248 | test_filter.root_covariance.diagonal().segment<3>(6).norm()*2); 249 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 250 | test_filter.root_covariance.diagonal().segment<3>(9).norm()*2); 251 | } 252 | 253 | TEST(SquareRootCoreTest, InnovationStep) { 254 | MyCore test_filter = create_initialised_sr_test_filter(); 255 | MyMeasurementVector m; 256 | 257 | m.set_field(UKF::Vector<3>(100, 10, -50)); 258 | m.set_field(UKF::Vector<3>(20, 0, 0)); 259 | m.set_field(UKF::Vector<3>(0, 0, -9.8)); 260 | m.set_field(UKF::FieldVector(0, -1, 0)); 261 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 262 | 263 | test_filter.a_priori_step(0.01); 264 | test_filter.innovation_step(m); 265 | 266 | /* 267 | With the field vector, we expect the determinant to be approximately zero, 268 | so allow for it to be slightly negative due to numerical precision. 269 | */ 270 | EXPECT_GE(test_filter.innovation_root_covariance.determinant(), -std::numeric_limits::epsilon()); 271 | 272 | test_filter.a_posteriori_step(); 273 | 274 | EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits::epsilon()); 275 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 276 | test_filter.root_covariance.diagonal().segment<3>(0).norm()*2); 277 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 278 | test_filter.root_covariance.diagonal().segment<3>(3).norm()*2); 279 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 280 | test_filter.root_covariance.diagonal().segment<3>(6).norm()*2); 281 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 282 | test_filter.root_covariance.diagonal().segment<3>(9).norm()*2); 283 | } 284 | 285 | TEST(SquareRootCoreTest, InnovationStepPartialMeasurement) { 286 | MyCore test_filter = create_initialised_sr_test_filter(); 287 | MyMeasurementVector m; 288 | 289 | m.set_field(UKF::Vector<3>(0, 0, -9.8)); 290 | m.set_field(UKF::FieldVector(0, -1, 0)); 291 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 292 | 293 | test_filter.a_priori_step(0.01); 294 | test_filter.innovation_step(m); 295 | 296 | EXPECT_GE(test_filter.innovation_root_covariance.determinant(), -std::numeric_limits::epsilon()); 297 | 298 | test_filter.a_posteriori_step(); 299 | 300 | EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits::epsilon()); 301 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 302 | test_filter.root_covariance.diagonal().segment<3>(0).norm()*2); 303 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 304 | test_filter.root_covariance.diagonal().segment<3>(3).norm()*2); 305 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 306 | test_filter.root_covariance.diagonal().segment<3>(6).norm()*2); 307 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 308 | test_filter.root_covariance.diagonal().segment<3>(9).norm()*2); 309 | } 310 | 311 | TEST(SquareRootCoreTest, InnovationStepWithInputs) { 312 | MyCore test_filter = create_initialised_sr_test_filter(); 313 | MyMeasurementVector m; 314 | 315 | m.set_field(UKF::Vector<3>(100, 10, -50)); 316 | m.set_field(UKF::Vector<3>(20, 0, 0)); 317 | m.set_field(UKF::Vector<3>(0, 0, -15)); 318 | m.set_field(UKF::FieldVector(0, -1, 0)); 319 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 320 | 321 | test_filter.a_priori_step(0.01, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0)); 322 | test_filter.innovation_step(m, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0)); 323 | 324 | EXPECT_GE(test_filter.innovation_root_covariance.determinant(), -std::numeric_limits::epsilon()); 325 | 326 | test_filter.a_posteriori_step(); 327 | 328 | EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits::epsilon()); 329 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 330 | test_filter.root_covariance.diagonal().segment<3>(0).norm()*2); 331 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 332 | test_filter.root_covariance.diagonal().segment<3>(3).norm()*2); 333 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 334 | test_filter.root_covariance.diagonal().segment<3>(6).norm()*2); 335 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 336 | test_filter.root_covariance.diagonal().segment<3>(9).norm()*2); 337 | } 338 | 339 | TEST(SquareRootCoreTest, InnovationStepPartialMeasurementWithInputs) { 340 | MyCore test_filter = create_initialised_sr_test_filter(); 341 | MyMeasurementVector m; 342 | 343 | m.set_field(UKF::Vector<3>(0, 0, -15)); 344 | m.set_field(UKF::FieldVector(0, -1, 0)); 345 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 346 | 347 | test_filter.a_priori_step(0.01, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0)); 348 | test_filter.innovation_step(m, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0)); 349 | 350 | EXPECT_GE(test_filter.innovation_root_covariance.determinant(), -std::numeric_limits::epsilon()); 351 | 352 | test_filter.a_posteriori_step(); 353 | 354 | EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits::epsilon()); 355 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 356 | test_filter.root_covariance.diagonal().segment<3>(0).norm()*2); 357 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 358 | test_filter.root_covariance.diagonal().segment<3>(3).norm()*2); 359 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 360 | test_filter.root_covariance.diagonal().segment<3>(6).norm()*2); 361 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 362 | test_filter.root_covariance.diagonal().segment<3>(9).norm()*2); 363 | } 364 | 365 | TEST(SquareRootCoreTest, APosterioriStep) { 366 | MyCore test_filter = create_initialised_sr_test_filter(); 367 | MyMeasurementVector m; 368 | 369 | m.set_field(UKF::Vector<3>(100, 10, -50)); 370 | m.set_field(UKF::Vector<3>(20, 0, 0)); 371 | m.set_field(UKF::Vector<3>(0, 0, -9.8)); 372 | m.set_field(UKF::FieldVector(0, -1, 0)); 373 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 374 | 375 | test_filter.a_priori_step(0.01); 376 | test_filter.innovation_step(m); 377 | test_filter.a_posteriori_step(); 378 | 379 | EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits::epsilon()); 380 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 381 | test_filter.root_covariance.diagonal().segment<3>(0).norm()*2); 382 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 383 | test_filter.root_covariance.diagonal().segment<3>(3).norm()*2); 384 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 385 | test_filter.root_covariance.diagonal().segment<3>(6).norm()*2); 386 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 387 | test_filter.root_covariance.diagonal().segment<3>(9).norm()*2); 388 | } 389 | 390 | TEST(SquareRootCoreTest, FullStep) { 391 | MyCore test_filter = create_initialised_sr_test_filter(); 392 | MyMeasurementVector m; 393 | 394 | m.set_field(UKF::Vector<3>(100, 10, -50)); 395 | m.set_field(UKF::Vector<3>(20, 0, 0)); 396 | m.set_field(UKF::Vector<3>(0, 0, -14.8)); 397 | m.set_field(UKF::FieldVector(0, -1, 0)); 398 | m.set_field(UKF::Vector<3>(0.5, 0, 0)); 399 | 400 | test_filter.step(0.01, m, UKF::Vector<3>(0, 0, -5), UKF::Vector<3>(1, 0, 0)); 401 | 402 | EXPECT_GT(test_filter.root_covariance.determinant(), std::numeric_limits::epsilon()); 403 | EXPECT_LT((UKF::Vector<3>(100, 10, -50) - test_filter.state.get_field()).norm(), 404 | test_filter.root_covariance.diagonal().segment<3>(0).norm()*2); 405 | EXPECT_LT((UKF::Vector<3>(20, 0, 0) - test_filter.state.get_field()).norm(), 406 | test_filter.root_covariance.diagonal().segment<3>(3).norm()*2); 407 | EXPECT_LT(2*std::acos(std::abs(UKF::Quaternion(0.7071, 0, 0, -0.7071).dot(test_filter.state.get_field()))), 408 | test_filter.root_covariance.diagonal().segment<3>(6).norm()*2); 409 | EXPECT_LT((UKF::Vector<3>(0.5, 0, 0) - test_filter.state.get_field()).norm(), 410 | test_filter.root_covariance.diagonal().segment<3>(9).norm()*2); 411 | } 412 | -------------------------------------------------------------------------------- /test/TestStateVector.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "UKF/Types.h" 5 | #include "UKF/Integrator.h" 6 | #include "UKF/StateVector.h" 7 | #include "comparisons.h" 8 | 9 | enum MyFields { 10 | LatLon, 11 | Altitude, 12 | Velocity, 13 | Acceleration, 14 | Attitude, 15 | AngularVelocity, 16 | Position 17 | }; 18 | 19 | using MyStateVector = UKF::StateVector< 20 | UKF::Field>, 21 | UKF::Field>, 22 | UKF::Field, 23 | UKF::Field 24 | >; 25 | 26 | TEST(StateVectorTest, Instantiation) { 27 | MyStateVector test_state; 28 | 29 | EXPECT_EQ(10, MyStateVector::MaxRowsAtCompileTime); 30 | EXPECT_EQ(10, test_state.size()); 31 | EXPECT_EQ(9, MyStateVector::CovarianceMatrix::MaxRowsAtCompileTime); 32 | EXPECT_EQ(9, MyStateVector::CovarianceMatrix::MaxColsAtCompileTime); 33 | EXPECT_EQ(10, MyStateVector::SigmaPointDistribution::MaxRowsAtCompileTime); 34 | EXPECT_EQ(19, MyStateVector::SigmaPointDistribution::MaxColsAtCompileTime); 35 | } 36 | 37 | TEST(StateVectorTest, Assignment) { 38 | MyStateVector test_state; 39 | 40 | test_state.set_field(UKF::Vector<2>(-37.8136, 144.9631)); 41 | test_state.set_field(UKF::Vector<3>(1, 2, 3)); 42 | test_state.set_field(UKF::Quaternion(1, 0, 0, 0)); 43 | test_state.set_field(10); 44 | 45 | EXPECT_VECTOR_EQ(UKF::Vector<2>(-37.8136, 144.9631), test_state.get_field()); 46 | EXPECT_VECTOR_EQ(UKF::Vector<3>(1, 2, 3), test_state.get_field()); 47 | EXPECT_QUATERNION_EQ(UKF::Quaternion(1, 0, 0, 0), test_state.get_field()); 48 | EXPECT_EQ(10, test_state.get_field()); 49 | } 50 | 51 | TEST(StateVectorTest, Arithmetic) { 52 | 53 | } 54 | 55 | TEST(StateVectorTest, DefaultParameters) { 56 | MyStateVector test_state; 57 | 58 | EXPECT_FLOAT_EQ(1.0, UKF::Parameters::AlphaSquared); 59 | EXPECT_FLOAT_EQ(0.0, UKF::Parameters::Beta); 60 | EXPECT_FLOAT_EQ(3.0, UKF::Parameters::Kappa); 61 | EXPECT_FLOAT_EQ(3.0, UKF::Parameters::Lambda); 62 | EXPECT_FLOAT_EQ(0.0, UKF::Parameters::MRP_A); 63 | EXPECT_FLOAT_EQ(2.0, UKF::Parameters::MRP_F); 64 | EXPECT_FLOAT_EQ(1.0 / 4.0, UKF::Parameters::Sigma_WM0); 65 | EXPECT_FLOAT_EQ(1.0 / 4.0, UKF::Parameters::Sigma_WC0); 66 | EXPECT_FLOAT_EQ(1.0 / 24.0, UKF::Parameters::Sigma_WMI); 67 | EXPECT_FLOAT_EQ(1.0 / 24.0, UKF::Parameters::Sigma_WCI); 68 | } 69 | 70 | using AlternateStateVector = UKF::StateVector< 71 | UKF::Field, 72 | UKF::Field>, 73 | UKF::Field> 74 | >; 75 | 76 | namespace UKF { 77 | namespace Parameters { 78 | template <> constexpr real_t AlphaSquared = 1e-6; 79 | template <> constexpr real_t Beta = 2.0; 80 | template <> constexpr real_t Kappa = 3.0; 81 | }} 82 | 83 | TEST(StateVectorTest, CustomParameters) { 84 | AlternateStateVector test_state; 85 | 86 | EXPECT_FLOAT_EQ(1e-6, UKF::Parameters::AlphaSquared); 87 | EXPECT_FLOAT_EQ(2.0, UKF::Parameters::Beta); 88 | EXPECT_FLOAT_EQ(3.0, UKF::Parameters::Kappa); 89 | EXPECT_FLOAT_EQ(12e-6 - 9, UKF::Parameters::Lambda); 90 | EXPECT_FLOAT_EQ(0.0, UKF::Parameters::MRP_A); 91 | EXPECT_FLOAT_EQ(2.0, UKF::Parameters::MRP_F); 92 | EXPECT_FLOAT_EQ(-749999, UKF::Parameters::Sigma_WM0); 93 | EXPECT_FLOAT_EQ(-749996, UKF::Parameters::Sigma_WC0); 94 | EXPECT_FLOAT_EQ(41666.667, UKF::Parameters::Sigma_WMI); 95 | EXPECT_FLOAT_EQ(41666.667, UKF::Parameters::Sigma_WCI); 96 | 97 | EXPECT_FLOAT_EQ(1.0, UKF::Parameters::AlphaSquared); 98 | EXPECT_FLOAT_EQ(0.0, UKF::Parameters::Beta); 99 | EXPECT_FLOAT_EQ(3.0, UKF::Parameters::Kappa); 100 | EXPECT_FLOAT_EQ(3.0, UKF::Parameters::Lambda); 101 | EXPECT_FLOAT_EQ(0.0, UKF::Parameters::MRP_A); 102 | EXPECT_FLOAT_EQ(2.0, UKF::Parameters::MRP_F); 103 | EXPECT_FLOAT_EQ(1.0 / 4.0, UKF::Parameters::Sigma_WM0); 104 | EXPECT_FLOAT_EQ(1.0 / 4.0, UKF::Parameters::Sigma_WC0); 105 | EXPECT_FLOAT_EQ(1.0 / 24.0, UKF::Parameters::Sigma_WMI); 106 | EXPECT_FLOAT_EQ(1.0 / 24.0, UKF::Parameters::Sigma_WCI); 107 | } 108 | 109 | TEST(StateVectorTest, SigmaPointGeneration) { 110 | MyStateVector test_state; 111 | 112 | test_state.set_field(UKF::Vector<2>(45, 135)); 113 | test_state.set_field(UKF::Vector<3>(1, 2, 3)); 114 | test_state.set_field(UKF::Quaternion(1, 0, 0, 0)); 115 | test_state.set_field(10); 116 | 117 | MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero(); 118 | covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; 119 | 120 | MyStateVector::SigmaPointDistribution sigma_points, target_sigma_points; 121 | 122 | target_sigma_points << 45, 48.464, 45, 45, 45, 45, 45, 45, 45, 45, 41.536, 45, 45, 45, 45, 45, 45, 45, 45, 123 | 135, 135, 138.46, 135, 135, 135, 135, 135, 135, 135, 135, 131.54, 135, 135, 135, 135, 135, 135, 135, 124 | 1, 1, 1, 4.464, 1, 1, 1, 1, 1, 1, 1, 1, -2.464, 1, 1, 1, 1, 1, 1, 125 | 2, 2, 2, 2, 5.464, 2, 2, 2, 2, 2, 2, 2, 2, -1.464, 2, 2, 2, 2, 2, 126 | 3, 3, 3, 3, 3, 6.464, 3, 3, 3, 3, 3, 3, 3, 3, -0.464, 3, 3, 3, 3, 127 | 0, 0, 0, 0, 0, 0, 0.866, 0, 0, 0, 0, 0, 0, 0, 0, -0.866, 0, 0, 0, 128 | 0, 0, 0, 0, 0, 0, 0, 0.866, 0, 0, 0, 0, 0, 0, 0, 0, -0.866, 0, 0, 129 | 0, 0, 0, 0, 0, 0, 0, 0, 0.866, 0, 0, 0, 0, 0, 0, 0, 0, -0.866, 0, 130 | 1, 1, 1, 1, 1, 1, 0.5, 0.5, 0.5, 1, 1, 1, 1, 1, 1, 0.5, 0.5, 0.5, 1, 131 | 10, 10, 10, 10, 10, 10, 10, 10, 10, 13.464, 10, 10, 10, 10, 10, 10, 10, 10, 6.536; 132 | sigma_points = test_state.calculate_sigma_point_distribution((covariance * 133 | (MyStateVector::covariance_size() + UKF::Parameters::Lambda)).llt().matrixL()); 134 | 135 | EXPECT_VECTOR_EQ(target_sigma_points.col(0), sigma_points.col(0)); 136 | EXPECT_VECTOR_EQ(target_sigma_points.col(1), sigma_points.col(1)); 137 | EXPECT_VECTOR_EQ(target_sigma_points.col(2), sigma_points.col(2)); 138 | EXPECT_VECTOR_EQ(target_sigma_points.col(3), sigma_points.col(3)); 139 | EXPECT_VECTOR_EQ(target_sigma_points.col(4), sigma_points.col(4)); 140 | EXPECT_VECTOR_EQ(target_sigma_points.col(5), sigma_points.col(5)); 141 | EXPECT_VECTOR_EQ(target_sigma_points.col(6), sigma_points.col(6)); 142 | EXPECT_VECTOR_EQ(target_sigma_points.col(7), sigma_points.col(7)); 143 | EXPECT_VECTOR_EQ(target_sigma_points.col(8), sigma_points.col(8)); 144 | EXPECT_VECTOR_EQ(target_sigma_points.col(9), sigma_points.col(9)); 145 | EXPECT_VECTOR_EQ(target_sigma_points.col(10), sigma_points.col(10)); 146 | EXPECT_VECTOR_EQ(target_sigma_points.col(11), sigma_points.col(11)); 147 | EXPECT_VECTOR_EQ(target_sigma_points.col(12), sigma_points.col(12)); 148 | EXPECT_VECTOR_EQ(target_sigma_points.col(13), sigma_points.col(13)); 149 | EXPECT_VECTOR_EQ(target_sigma_points.col(14), sigma_points.col(14)); 150 | EXPECT_VECTOR_EQ(target_sigma_points.col(15), sigma_points.col(15)); 151 | EXPECT_VECTOR_EQ(target_sigma_points.col(16), sigma_points.col(16)); 152 | EXPECT_VECTOR_EQ(target_sigma_points.col(17), sigma_points.col(17)); 153 | EXPECT_VECTOR_EQ(target_sigma_points.col(18), sigma_points.col(18)); 154 | } 155 | 156 | TEST(StateVectorTest, SigmaPointMean) { 157 | MyStateVector test_state; 158 | 159 | test_state.set_field(UKF::Vector<2>(-37.8136, 144.9631)); 160 | test_state.set_field(UKF::Vector<3>(1, 2, 3)); 161 | test_state.set_field(UKF::Quaternion(1, 0, 0, 0)); 162 | test_state.set_field(10); 163 | 164 | MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero(); 165 | covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; 166 | 167 | MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance * 168 | (MyStateVector::covariance_size() + UKF::Parameters::Lambda)).llt().matrixL()); 169 | 170 | EXPECT_VECTOR_EQ(test_state, MyStateVector::calculate_sigma_point_mean(sigma_points)); 171 | } 172 | 173 | TEST(StateVectorTest, SigmaPointDeltas) { 174 | MyStateVector test_state; 175 | 176 | test_state.set_field(UKF::Vector<2>(-37.8136, 144.9631)); 177 | test_state.set_field(UKF::Vector<3>(1, 2, 3)); 178 | test_state.set_field(UKF::Quaternion(1, 0, 0, 0)); 179 | test_state.set_field(10); 180 | 181 | MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero(); 182 | covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; 183 | 184 | MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance * 185 | (MyStateVector::covariance_size() + UKF::Parameters::Lambda)).llt().matrixL()); 186 | MyStateVector test_mean = MyStateVector::calculate_sigma_point_mean(sigma_points); 187 | MyStateVector::SigmaPointDeltas sigma_point_deltas, target_sigma_point_deltas; 188 | 189 | target_sigma_point_deltas << 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0, 0, 0, 0, 0, 0, 0, 0, 190 | 0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0, 0, 0, 0, 0, 0, 0, 191 | 0, 0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0, 0, 0, 0, 0, 0, 192 | 0, 0, 0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0, 0, 0, 0, 0, 193 | 0, 0, 0, 0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0, 0, 0, 0, 194 | 0, 0, 0, 0, 0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0, 0, 0, 195 | 0, 0, 0, 0, 0, 0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0, 0, 196 | 0, 0, 0, 0, 0, 0, 0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0, 197 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464; 198 | sigma_point_deltas = test_mean.calculate_sigma_point_deltas(sigma_points); 199 | 200 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(0), sigma_point_deltas.col(0)); 201 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(1), sigma_point_deltas.col(1)); 202 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(2), sigma_point_deltas.col(2)); 203 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(3), sigma_point_deltas.col(3)); 204 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(4), sigma_point_deltas.col(4)); 205 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(5), sigma_point_deltas.col(5)); 206 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(6), sigma_point_deltas.col(6)); 207 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(7), sigma_point_deltas.col(7)); 208 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(8), sigma_point_deltas.col(8)); 209 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(9), sigma_point_deltas.col(9)); 210 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(10), sigma_point_deltas.col(10)); 211 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(11), sigma_point_deltas.col(11)); 212 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(12), sigma_point_deltas.col(12)); 213 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(13), sigma_point_deltas.col(13)); 214 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(14), sigma_point_deltas.col(14)); 215 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(15), sigma_point_deltas.col(15)); 216 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(16), sigma_point_deltas.col(16)); 217 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(17), sigma_point_deltas.col(17)); 218 | EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(18), sigma_point_deltas.col(18)); 219 | } 220 | 221 | TEST(StateVectorTest, SigmaPointCovariance) { 222 | MyStateVector test_state; 223 | 224 | test_state.set_field(UKF::Vector<2>(-37.8136, 144.9631)); 225 | test_state.set_field(UKF::Vector<3>(1, 2, 3)); 226 | test_state.set_field(UKF::Quaternion(1, 0, 0, 0)); 227 | test_state.set_field(10); 228 | 229 | MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero(); 230 | covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; 231 | 232 | MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance * 233 | (MyStateVector::covariance_size() + UKF::Parameters::Lambda)).llt().matrixL()); 234 | MyStateVector test_mean = MyStateVector::calculate_sigma_point_mean(sigma_points); 235 | MyStateVector::SigmaPointDeltas sigma_point_deltas = test_mean.calculate_sigma_point_deltas(sigma_points); 236 | MyStateVector::CovarianceMatrix calculated_covariance = MyStateVector::calculate_sigma_point_covariance(sigma_point_deltas); 237 | 238 | EXPECT_VECTOR_EQ(covariance.col(0), calculated_covariance.col(0)); 239 | EXPECT_VECTOR_EQ(covariance.col(1), calculated_covariance.col(1)); 240 | EXPECT_VECTOR_EQ(covariance.col(2), calculated_covariance.col(2)); 241 | EXPECT_VECTOR_EQ(covariance.col(3), calculated_covariance.col(3)); 242 | EXPECT_VECTOR_EQ(covariance.col(4), calculated_covariance.col(4)); 243 | EXPECT_VECTOR_EQ(covariance.col(5), calculated_covariance.col(5)); 244 | EXPECT_VECTOR_EQ(covariance.col(6), calculated_covariance.col(6)); 245 | EXPECT_VECTOR_EQ(covariance.col(7), calculated_covariance.col(7)); 246 | EXPECT_VECTOR_EQ(covariance.col(8), calculated_covariance.col(8)); 247 | } 248 | 249 | TEST(StateVectorTest, SmallAlphaSigmaPoints) { 250 | AlternateStateVector test_state; 251 | 252 | test_state.set_field(UKF::Quaternion(1, 0, 0, 0)); 253 | test_state.set_field(UKF::Vector<3>(1, 2, 3)); 254 | test_state.set_field(UKF::Vector<3>(1, 2, 3)); 255 | 256 | AlternateStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero(); 257 | covariance.diagonal() << 1000.0, 1000.0, 1000.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; 258 | 259 | AlternateStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance * 260 | (AlternateStateVector::covariance_size() + UKF::Parameters::Lambda)).llt().matrixL()); 261 | AlternateStateVector test_mean = AlternateStateVector::calculate_sigma_point_mean(sigma_points); 262 | AlternateStateVector::SigmaPointDeltas sigma_point_deltas = test_mean.calculate_sigma_point_deltas(sigma_points); 263 | AlternateStateVector::CovarianceMatrix calculated_covariance = AlternateStateVector::calculate_sigma_point_covariance(sigma_point_deltas); 264 | 265 | EXPECT_VECTOR_EQ(covariance.col(0), calculated_covariance.col(0)); 266 | EXPECT_VECTOR_EQ(covariance.col(1), calculated_covariance.col(1)); 267 | EXPECT_VECTOR_EQ(covariance.col(2), calculated_covariance.col(2)); 268 | EXPECT_VECTOR_EQ(covariance.col(3), calculated_covariance.col(3)); 269 | EXPECT_VECTOR_EQ(covariance.col(4), calculated_covariance.col(4)); 270 | EXPECT_VECTOR_EQ(covariance.col(5), calculated_covariance.col(5)); 271 | EXPECT_VECTOR_EQ(covariance.col(6), calculated_covariance.col(6)); 272 | EXPECT_VECTOR_EQ(covariance.col(7), calculated_covariance.col(7)); 273 | EXPECT_VECTOR_EQ(covariance.col(8), calculated_covariance.col(8)); 274 | 275 | EXPECT_QUATERNION_EQ(test_state.get_field(), test_mean.get_field()); 276 | EXPECT_VECTOR_EQ(test_state.get_field(), test_mean.get_field()); 277 | EXPECT_VECTOR_EQ(test_state.get_field(), test_mean.get_field()); 278 | } 279 | 280 | /* 281 | These are linear, but they don't have to be – just makes it easier to 282 | calculate the predicted output. 283 | */ 284 | 285 | using ProcessModelTestStateVector = UKF::StateVector< 286 | UKF::Field>, 287 | UKF::Field> 288 | >; 289 | 290 | namespace UKF { 291 | template <> template <> 292 | ProcessModelTestStateVector ProcessModelTestStateVector::derivative<>() const { 293 | ProcessModelTestStateVector temp; 294 | /* Position derivative. */ 295 | temp.set_field(get_field()); 296 | 297 | /* Velocity derivative. */ 298 | temp.set_field(UKF::Vector<3>(0, 0, 0)); 299 | 300 | return temp; 301 | } 302 | 303 | template <> template <> 304 | ProcessModelTestStateVector ProcessModelTestStateVector::derivative>( 305 | const UKF::Vector<3>& acceleration) const { 306 | ProcessModelTestStateVector temp; 307 | /* Position derivative. */ 308 | temp.set_field(get_field()); 309 | 310 | /* Velocity derivative. */ 311 | temp.set_field(acceleration); 312 | 313 | return temp; 314 | } 315 | 316 | } 317 | 318 | TEST(StateVectorTest, ProcessModel) { 319 | ProcessModelTestStateVector test_state; 320 | UKF::Vector<6> expected_state; 321 | 322 | test_state.set_field(UKF::Vector<3>(0, 0, 0)); 323 | test_state.set_field(UKF::Vector<3>(1, 2, 3)); 324 | 325 | expected_state << 0.1, 0.2, 0.3, 1, 2, 3; 326 | EXPECT_VECTOR_EQ(expected_state, test_state.process_model(0.1)); 327 | EXPECT_VECTOR_EQ(expected_state, test_state.process_model(0.1)); 328 | EXPECT_VECTOR_EQ(expected_state, test_state.process_model(0.1)); 329 | EXPECT_VECTOR_EQ(expected_state, test_state.process_model(0.1)); 330 | 331 | expected_state << 0.1 + (0.5 * 3 * 0.1 * 0.1), 0.2 + (0.5 * 2 * 0.1 * 0.1), 0.3 + (0.5 * 1 * 0.1 * 0.1), 1.3, 2.2, 3.1; 332 | EXPECT_VECTOR_EQ(expected_state, test_state.process_model(0.1, UKF::Vector<3>(3, 2, 1))); 333 | EXPECT_VECTOR_EQ(expected_state, test_state.process_model(0.1, UKF::Vector<3>(3, 2, 1))); 334 | EXPECT_VECTOR_EQ(expected_state, test_state.process_model(0.1, UKF::Vector<3>(3, 2, 1))); 335 | expected_state << 0.1, 0.2, 0.3, 1.3, 2.2, 3.1; 336 | EXPECT_VECTOR_EQ(expected_state, test_state.process_model(0.1, UKF::Vector<3>(3, 2, 1))); 337 | } 338 | 339 | TEST(StateVectorTest, UpdateDelta) { 340 | MyStateVector test_state; 341 | 342 | test_state.set_field(UKF::Vector<2>(45, 135)); 343 | test_state.set_field(UKF::Vector<3>(1, 2, 3)); 344 | test_state.set_field(UKF::Quaternion(1, 0, 0, 0)); 345 | test_state.set_field(10); 346 | 347 | MyStateVector::StateVectorDelta test_delta; 348 | test_delta << 10, -20, 3, 2, 1, 0, 0, 0.5, 1; 349 | 350 | test_state.apply_delta(test_delta); 351 | 352 | MyStateVector expected_state; 353 | expected_state.set_field(UKF::Vector<2>(55, 115)); 354 | expected_state.set_field(UKF::Vector<3>(4, 4, 4)); 355 | expected_state.set_field(UKF::Quaternion(0.9692, 0, 0, 0.2462)); 356 | expected_state.set_field(11); 357 | 358 | EXPECT_VECTOR_EQ(expected_state, test_state); 359 | } 360 | -------------------------------------------------------------------------------- /test/comparisons.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | template 4 | inline ::testing::AssertionResult CmpHelperQuaternionEq( 5 | const char* expected_expression, const char* actual_expression, 6 | Q1 a, Q2 b, real_t tol = 0.02) { 7 | a.normalize(); 8 | b.normalize(); 9 | 10 | real_t angle = (real_t)2.0 * acos(std::abs(a.dot(b))); 11 | 12 | if (angle < tol) { 13 | return ::testing::AssertionSuccess(); 14 | } else { 15 | return ::testing::AssertionFailure() << 16 | "Value of: " << actual_expression << "\n" 17 | "Expected: " << expected_expression << "\n" << 18 | " Actual: Quaternion(" << b.w() << ", " << b.x() << ", " << 19 | b.y() << ", " << b.z() << ")\n" << 20 | " Error: " << angle << " rad\n"; 21 | } 22 | } 23 | 24 | template 25 | inline ::testing::AssertionResult CmpHelperVectorEq( 26 | const char* expected_expression, const char* actual_expression, 27 | T1 a, T2 b, real_t tol = 0.0001) { 28 | real_t d = (a - b).norm(), an = a.norm(); 29 | 30 | Eigen::IOFormat CmpFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "Vector(", ")"); 31 | 32 | if (d / std::max(an, (real_t)1e-4) < tol) { 33 | return ::testing::AssertionSuccess(); 34 | } else { 35 | return ::testing::AssertionFailure() << 36 | "Value of: " << actual_expression << "\n" 37 | "Expected: " << a.format(CmpFormat) << "\n" << 38 | " Actual: " << b.format(CmpFormat) << "\n"; 39 | } 40 | } 41 | 42 | #define EXPECT_QUATERNION_EQ(expected, actual)\ 43 | EXPECT_PRED_FORMAT2(CmpHelperQuaternionEq, expected, actual) 44 | 45 | #define EXPECT_VECTOR_EQ(expected, actual)\ 46 | EXPECT_PRED_FORMAT2(CmpHelperVectorEq, expected, actual) 47 | --------------------------------------------------------------------------------