├── .clang-format ├── .gitignore ├── .travis.yml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── FindEigen.cmake ├── FindEigen3.cmake ├── definitions.cmake ├── doxygen.cmake ├── gtest.cmake ├── info.cmake ├── sync_doc.cmake.in ├── target_failed.cmake.in └── version.cmake ├── doc ├── Doxyfile.in ├── bibliography.bib ├── filter_concept.dox ├── filter_definition.dox ├── filter_usage.dox ├── getting_started.dox ├── howto.dox ├── images │ ├── bayes.jpg │ └── filter_interface.svg ├── implement_filter.dox ├── mainpage.dox ├── modules.dox ├── theme │ ├── customization.css │ ├── header.html │ ├── layout.xml │ └── navtree_expand_selected.js └── working_with_fl.dox ├── include └── fl │ ├── distribution │ ├── cauchy_distribution.hpp │ ├── chi_squared.hpp │ ├── decorrelated_gaussian.hpp │ ├── discrete_distribution.hpp │ ├── exponential_distribution.hpp │ ├── gaussian.hpp │ ├── interface │ │ ├── approximate_moments.hpp │ │ ├── evaluation.hpp │ │ ├── moments.hpp │ │ ├── sampling.hpp │ │ ├── standard_gaussian_mapping.hpp │ │ └── unnormalized_evaluation.hpp │ ├── joint_distribution.hpp │ ├── joint_distribution_id.hpp │ ├── joint_distribution_iid.hpp │ ├── standard_gaussian.hpp │ ├── t_distribution.hpp │ ├── truncated_gaussian.hpp │ └── uniform_distribution.hpp │ ├── exception │ └── exception.hpp │ ├── filter │ ├── filter_interface.hpp │ ├── gaussian │ │ ├── X_robust_multi_sensor_gaussian_filter.hpp │ │ ├── gaussian_filter.hpp │ │ ├── gaussian_filter_linear.hpp │ │ ├── gaussian_filter_nonlinear.hpp │ │ ├── gaussian_filter_nonlinear_generic.hpp │ │ ├── multi_sensor_gaussian_filter.hpp │ │ ├── prediction_policy │ │ │ ├── sigma_point_additive_prediction_policy.hpp │ │ │ └── sigma_point_prediction_policy.hpp │ │ ├── quadrature │ │ │ ├── sigma_point_quadrature.hpp │ │ │ └── unscented_quadrature.hpp │ │ ├── robust_gaussian_filter.hpp │ │ ├── robust_multi_sensor_gaussian_filter.hpp │ │ ├── transform │ │ │ ├── monte_carlo_transform.hpp │ │ │ ├── point_set.hpp │ │ │ ├── point_set_transform.hpp │ │ │ └── unscented_transform.hpp │ │ └── update_policy │ │ │ ├── multi_sensor_sigma_point_update_policy.hpp │ │ │ ├── multi_sensor_sigma_point_update_polizzle.hpp │ │ │ ├── sigma_point_additive_uncorrelated_update_policy.hpp │ │ │ ├── sigma_point_additive_update_policy.hpp │ │ │ └── sigma_point_update_policy.hpp │ └── particle │ │ └── particle_filter.hpp │ ├── model │ ├── adaptive_model.hpp │ ├── additive_noise_model.hpp │ ├── additive_uncorrelated_noise_model.hpp │ ├── sensor │ │ ├── body_tail_sensor.hpp │ │ ├── interface │ │ │ ├── additive_sensor_function.hpp │ │ │ ├── additive_uncorrelated_sensor_function.hpp │ │ │ ├── sensor_density.hpp │ │ │ └── sensor_function.hpp │ │ ├── joint_sensor.hpp │ │ ├── joint_sensor_id.hpp │ │ ├── joint_sensor_iid.hpp │ │ ├── linear_cauchy_sensor.hpp │ │ ├── linear_decorrelated_gaussian_sensor.hpp │ │ ├── linear_gaussian_sensor.hpp │ │ ├── linear_sensor.hpp │ │ ├── multi_robust_sensor_function.hpp │ │ ├── robust_sensor_function.hpp │ │ └── uniform_sensor.hpp │ └── transition │ │ ├── binary_transition_density.hpp │ │ ├── interface │ │ ├── additive_transition_function.hpp │ │ ├── additive_uncorrelated_transition_function.hpp │ │ ├── transition_density.hpp │ │ └── transition_function.hpp │ │ ├── joint_transition.hpp │ │ ├── joint_transition_id.hpp │ │ ├── joint_transition_iid.hpp │ │ └── linear_transition.hpp │ └── util │ ├── assertions.hpp │ ├── descriptor.hpp │ ├── math.hpp │ ├── math │ ├── general_functions.hpp │ ├── linear_algebra.hpp │ └── special_functions.hpp │ ├── meta.hpp │ ├── meta │ ├── index_sequence.hpp │ ├── operator │ │ ├── adaptive.hpp │ │ ├── forward_adaptive.hpp │ │ ├── join.hpp │ │ ├── multiple_of.hpp │ │ ├── not_adaptive.hpp │ │ └── use_as.hpp │ ├── options_argument.hpp │ ├── size_deduction.hpp │ └── type_sequence.hpp │ ├── profiling.hpp │ ├── random.hpp │ ├── scalar_matrix.hpp │ ├── traits.hpp │ └── types.hpp ├── package.xml ├── templates ├── filter.template.hpp └── templated_filter.template.hpp └── test ├── CMakeLists.txt ├── distribution ├── chi_squared_distribution_test.cpp ├── decorrelated_gaussian_test.cpp ├── discrete_distribution_test.cpp ├── gaussian_test.cpp ├── joint_distribution_id_test.cpp ├── joint_distribution_iid_test.cpp └── t_distribution_test.cpp ├── exception └── exception_test.cpp ├── filter_interface ├── filter_interface_stubs.hpp └── filter_interface_test.cpp ├── gaussian_filter ├── CMakeLists.txt ├── gaussian_filter.cmake ├── gaussian_filter_linear_vs_unscented_kalman_filter_test.cpp ├── gaussian_filter_linear_vs_unscented_kalman_filter_test.cpp.in ├── gaussian_filter_linear_vs_unscented_kalman_filter_test.hpp ├── gaussian_filter_linear_vs_x_test_suite.hpp ├── gaussian_filter_stubs.hpp ├── gaussian_filter_test.cpp ├── gaussian_filter_test_suite.hpp ├── kalman_filter_test.cpp ├── multi_sensor_gaussian_filter_test.cpp ├── point_set_test.cpp ├── robust_gaussian_filter_test.cpp ├── robust_multi_sensor_gaussian_filter_test.cpp ├── sigma_point_quadrature.cmake ├── sigma_point_quadrature_test.cpp.in ├── sigma_point_quadrature_test.hpp ├── unscented_kalman_filter_test.cpp └── unscented_transform_test.cpp ├── gtest_main.cpp ├── model ├── binary_transition_density_test.cpp ├── joint_sensor_iid_test.cpp ├── joint_transition_id_test.cpp ├── joint_transition_iid_test.cpp ├── sensor │ ├── body_tail_sensor_test.cpp │ ├── joint_sensor_iid_test.cpp │ ├── linear_gaussian_sensor_test.cpp │ └── linear_uncorrelated_gaussian_sensor_test.cpp └── transition │ ├── linear_transition_test.cpp │ └── orientation_transition_function_test.cpp ├── particle_filter ├── particle_filter_fixture_test.cpp └── particle_filter_test.cpp ├── typecast.hpp └── utils ├── descriptor_test.cpp ├── join_test.cpp ├── linear_algebra_is_diagonal_test.cpp ├── linear_algebra_smw_inversion_test.cpp ├── linear_algebra_square_root_test.cpp ├── meta_test.cpp ├── not_adaptive_test.cpp ├── options_test.cpp ├── scalar_matrix_test.cpp ├── some_tests.cpp ├── special_functions_normal_to_uniform_test.cpp └── traits_test.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | # custom settings 3 | Language: Cpp 4 | Standard: Cpp11 5 | IndentWidth: 4 6 | TabWidth: 4 7 | AccessModifierOffset: -4 8 | AllowShortFunctionsOnASingleLine: Inline 9 | AllowShortIfStatementsOnASingleLine: true 10 | AllowShortLoopsOnASingleLine: true 11 | BinPackArguments: false 12 | BinPackParameters: false 13 | BreakBeforeBraces: Allman 14 | PointerAlignment: Left 15 | 16 | # BasedOnStyle: Chromium 17 | DerivePointerAlignment: false 18 | AlignAfterOpenBracket: true 19 | AlignConsecutiveAssignments: false 20 | AlignEscapedNewlinesLeft: true 21 | AlignOperands: true 22 | AlignTrailingComments: true 23 | AllowAllParametersOfDeclarationOnNextLine: false 24 | AllowShortBlocksOnASingleLine: false 25 | AllowShortCaseLabelsOnASingleLine: false 26 | AlwaysBreakAfterDefinitionReturnType: None 27 | AlwaysBreakBeforeMultilineStrings: true 28 | AlwaysBreakTemplateDeclarations: true 29 | BreakBeforeBinaryOperators: None 30 | BreakBeforeTernaryOperators: true 31 | BreakConstructorInitializersBeforeComma: false 32 | ColumnLimit: 80 33 | CommentPragmas: '^ IWYU pragma:' 34 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 35 | ConstructorInitializerIndentWidth: 4 36 | ContinuationIndentWidth: 4 37 | Cpp11BracedListStyle: true 38 | DisableFormat: false 39 | ExperimentalAutoDetectBinPacking: false 40 | ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] 41 | IndentCaseLabels: true 42 | IndentWrappedFunctionNames: false 43 | KeepEmptyLinesAtTheStartOfBlocks: false 44 | MacroBlockBegin: '^IPC_END_MESSAGE_MAP$' 45 | MacroBlockEnd: '' 46 | MaxEmptyLinesToKeep: 1 47 | NamespaceIndentation: None 48 | ObjCBlockIndentWidth: 2 49 | ObjCSpaceAfterProperty: false 50 | ObjCSpaceBeforeProtocolList: false 51 | PenaltyBreakBeforeFirstCallParameter: 1 52 | PenaltyBreakComment: 300 53 | PenaltyBreakFirstLessLess: 120 54 | PenaltyBreakString: 1000 55 | PenaltyExcessCharacter: 1000000 56 | PenaltyReturnTypeOnItsOwnLine: 200 57 | SpaceAfterCStyleCast: false 58 | SpaceBeforeAssignmentOperators: true 59 | SpaceBeforeParens: ControlStatements 60 | SpaceInEmptyParentheses: false 61 | SpacesBeforeTrailingComments: 2 62 | SpacesInAngles: false 63 | SpacesInContainerLiterals: true 64 | SpacesInCStyleCastParentheses: false 65 | SpacesInParentheses: false 66 | SpacesInSquareBrackets: false 67 | UseTab: Never 68 | ... 69 | 70 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeLists.txt.user 2 | *.*~ 3 | build/* -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | 3 | compiler: 4 | - gcc 5 | 6 | before_install: 7 | - sudo apt-get update 8 | - sudo apt-get install -y software-properties-common 9 | - sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test 10 | - sudo apt-get update 11 | - sudo apt-get install -y gcc-4.8 g++-4.8 --no-install-recommends 12 | - sudo apt-get install libboost-all-dev 13 | 14 | branches: 15 | only: 16 | - master 17 | 18 | script: 19 | - hg clone https://bitbucket.org/eigen/eigen eigen && cd eigen && hg up 3.2.5 && mkdir build && cd build && cmake .. && sudo make install 20 | - cd .. 21 | - cd .. 22 | - mkdir build && cd build 23 | - cmake .. -DCMAKE_CXX_COMPILER=g++-4.8 -DCMAKE_CXX_FLAGS="-DEIGEN_STACK_ALLOCATION_LIMIT=0" 24 | - make -j1 25 | - make test 26 | 27 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(fl) 3 | 4 | ############################ 5 | # Options # 6 | ############################ 7 | option(fl_USE_CATKIN "Use catkin build system" ON) 8 | option(fl_USE_RANDOM_SEED "Use random seeds for number generators" ON) 9 | set(fl_FLOATING_POINT_TYPE "double" CACHE STRING "fl::Real floating point type") 10 | 11 | ############################ 12 | # Flags # 13 | ############################ 14 | # Enable c++11 GCC 4.6 or greater required 15 | add_definitions(-DEIGEN_STACK_ALLOCATION_LIMIT=1638400) 16 | add_definitions(-DEIGEN_MPL2_ONLY=1) 17 | add_definitions(-std=c++0x -fno-omit-frame-pointer) 18 | add_definitions(-DPROFILING_ON=1) #print profiling output 19 | 20 | add_definitions(-Wall) 21 | add_definitions(-Wno-unused-local-typedefs) 22 | add_definitions(-Wno-deprecated-declarations) 23 | add_definitions(-Wno-comment) 24 | 25 | # for eigen-3.1.2 26 | add_definitions(-Wno-deprecated-register) 27 | 28 | ############################ 29 | # Setup # 30 | ############################ 31 | #set(fl_INCLUDE_DIRS include) 32 | set(fl_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 33 | set(CMAKE_MODULE_PATH ${fl_MODULE_PATH}) 34 | include(${fl_MODULE_PATH}/definitions.cmake) 35 | 36 | ############################ 37 | # Exports ## 38 | ############################ 39 | #set(fl_FOUND TRUE PARENT_SCOPE) 40 | #set(fl_INCLUDE_DIRS ../fl/include ${Eigen_INCLUDE_DIRS} PARENT_SCOPE) 41 | #set(fl_DEFINITIONS ${Eigen_DEFINITIONS} PARENT_SCOPE) 42 | #set(fl_DEPENDS Eigen PARENT_SCOPE) 43 | 44 | ############################ 45 | # Library Version # 46 | ############################ 47 | #include(${fl_MODULE_PATH}/version.cmake) 48 | 49 | ############################ 50 | # Dependencies ## 51 | ############################ 52 | find_package(Eigen REQUIRED) 53 | include_directories(${Eigen_INCLUDE_DIRS}) 54 | add_definitions(${Eigen_DEFINITIONS}) 55 | 56 | find_package(Boost REQUIRED) 57 | include_directories(${Boost_INCLUDE_DIRS}) 58 | 59 | #find_package(OpenMP) 60 | #if(OPENMP_FOUND) 61 | # set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 62 | # set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 63 | # message (WARNING "${OpenMP_C_FLAGS} ${OpenMP_CXX_FLAGS}") 64 | #endif() 65 | 66 | ############################ 67 | ## catkin # 68 | ############################ 69 | if(fl_USE_CATKIN) 70 | find_package(catkin REQUIRED) 71 | endif(fl_USE_CATKIN) 72 | 73 | if(catkin_FOUND AND fl_USE_CATKIN) 74 | set(fl_USING_CATKIN YES) 75 | catkin_package(INCLUDE_DIRS include DEPENDS Eigen Boost) 76 | else(catkin_FOUND AND fl_USE_CATKIN) 77 | set(fl_USING_CATKIN NO) 78 | endif(catkin_FOUND AND fl_USE_CATKIN) 79 | 80 | ############################ 81 | # Documentation Generation # 82 | ############################ 83 | # 84 | # How to generate the documentation: 85 | # 86 | # $ cd /path/to/fl 87 | # $ mkdir build 88 | # $ cd build 89 | # $ cmake .. 90 | # $ make doc_fl 91 | # 92 | # The documentation will be generated within /path/to/fl/build/doc 93 | # 94 | set(MIN_DOXYGEN_VERSION 1.8.4) 95 | find_package(Doxygen ${MIN_DOXYGEN_VERSION}) 96 | include(${fl_MODULE_PATH}/doxygen.cmake) 97 | 98 | ############################ 99 | # Library info summary # 100 | ############################ 101 | #include(${fl_MODULE_PATH}/info.cmake) 102 | 103 | #info_begin() 104 | # info_project("::fl:: Filtering Library" ${PROJECT_VERSION}) 105 | # info_header("Setup:") 106 | # info_item("Using Catkin" "${fl_USING_CATKIN}") 107 | # info_item("Using random seed" "${fl_USE_RANDOM_SEED}") 108 | # info_item("Using fl::Real floating point type" ${fl_FLOATING_POINT_TYPE}) 109 | #info_end() 110 | 111 | ############################ 112 | # Build # 113 | ############################ 114 | include_directories(include) 115 | file(GLOB_RECURSE header_files include/${PROJECT_NAME}/*.hpp 116 | include/${PROJECT_NAME}/*.h 117 | include/ff/*.hpp 118 | include/ff/*.h) 119 | 120 | add_library(${PROJECT_NAME} ${header_files}) 121 | set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) 122 | 123 | ############################ 124 | # Tests # 125 | ############################ 126 | enable_testing() 127 | include(${fl_MODULE_PATH}/gtest.cmake) 128 | add_subdirectory(test) 129 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2014 Jan Issac, Manuel Wuethrich 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Bayesian Filtering Library (fl) 2 | =============== 3 | [![Build Status](https://travis-ci.org/filtering-library/fl.svg?branch=master)](https://travis-ci.org/filtering-library/fl) 4 | 5 | Documentation: [fl v0.1.2-rc](http://fl.tuxfamily.org) 6 | -------------------------------------------------------------------------------- /cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # 3 | # CMake script for finding the Eigen library. 4 | # 5 | # http://eigen.tuxfamily.org/index.php?title=Main_Page 6 | # 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Copyright (c) 2008, 2009 Gael Guennebaud, 9 | # Copyright (c) 2009 Benoit Jacob 10 | # Redistribution and use is allowed according to the terms of the 2-clause BSD 11 | # license. 12 | # 13 | # 14 | # Input variables: 15 | # 16 | # - Eigen_ROOT_DIR (optional): When specified, header files and libraries 17 | # will be searched for in `${Eigen_ROOT_DIR}/include` and 18 | # `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order 19 | # will be ignored. When unspecified, the default CMake search order is used. 20 | # This variable can be specified either as a CMake or environment variable. 21 | # If both are set, preference is given to the CMake variable. 22 | # Use this variable for finding packages installed in a nonstandard location, 23 | # or for enforcing that one of multiple package installations is picked up. 24 | # 25 | # Cache variables (not intended to be used in CMakeLists.txt files) 26 | # 27 | # - Eigen_INCLUDE_DIR: Absolute path to package headers. 28 | # 29 | # 30 | # Output variables: 31 | # 32 | # - Eigen_FOUND: Boolean that indicates if the package was found 33 | # - Eigen_INCLUDE_DIRS: Paths to the necessary header files 34 | # - Eigen_VERSION: Version of Eigen library found 35 | # - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen 36 | # 37 | # 38 | # Example usage: 39 | # 40 | # # Passing the version means Eigen_FOUND will only be TRUE if a 41 | # # version >= the provided version is found. 42 | # find_package(Eigen 3.1.2) 43 | # if(NOT Eigen_FOUND) 44 | # # Error handling 45 | # endif() 46 | # ... 47 | # add_definitions(${Eigen_DEFINITIONS}) 48 | # ... 49 | # include_directories(${Eigen_INCLUDE_DIRS} ...) 50 | # 51 | ############################################################################### 52 | 53 | find_package(PkgConfig) 54 | pkg_check_modules(PC_EIGEN eigen3) 55 | set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) 56 | 57 | 58 | find_path(EIGEN_INCLUDE_DIR Eigen/Core 59 | HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} 60 | "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" 61 | "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility 62 | PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" 63 | "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" 64 | PATH_SUFFIXES eigen3 include/eigen3 include) 65 | 66 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 67 | 68 | include(FindPackageHandleStandardArgs) 69 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) 70 | 71 | mark_as_advanced(EIGEN_INCLUDE_DIR) 72 | 73 | if(EIGEN_FOUND) 74 | message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") 75 | endif(EIGEN_FOUND) 76 | 77 | 78 | set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 79 | set(Eigen_FOUND ${EIGEN_FOUND}) 80 | set(Eigen_VERSION ${EIGEN_VERSION}) 81 | set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) 82 | -------------------------------------------------------------------------------- /cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | 82 | -------------------------------------------------------------------------------- /cmake/definitions.cmake: -------------------------------------------------------------------------------- 1 | ########################################################## 2 | # adds the definitions 3 | # 4 | # fl_USE_RANDOM_SEED 5 | # fl_USE_FLOAT OR fl_USE_DOUBLE OR fl_USE_LONG_DOUBLE 6 | ########################################################## 7 | 8 | if(fl_USE_RANDOM_SEED) 9 | add_definitions(-Dfl_USE_RANDOM_SEED=1) 10 | endif(fl_USE_RANDOM_SEED) 11 | 12 | 13 | if(fl_FLOATING_POINT_TYPE STREQUAL "float") 14 | add_definitions(-Dfl_USE_FLOAT=1) 15 | elseif(fl_FLOATING_POINT_TYPE STREQUAL "double") 16 | add_definitions(-Dfl_USE_DOUBLE=1) 17 | elseif(fl_FLOATING_POINT_TYPE STREQUAL "long double") 18 | add_definitions(-Dfl_USE_LONG_DOUBLE=1) 19 | else(fl_FLOATING_POINT_TYPE STREQUAL "float") 20 | if(fl_FLOATING_POINT_TYPE) 21 | message(WARNING 22 | "Unknown floating point type " 23 | "fl_FLOATING_POINT_TYPE=${fl_FLOATING_POINT_TYPE}. " 24 | "Falling back to default (double)") 25 | set(fl_FLOATING_POINT_TYPE "double" 26 | CACHE STRING "fl::Real floating point type" FORCE) 27 | endif(fl_FLOATING_POINT_TYPE) 28 | 29 | add_definitions(-Dfl_USE_DOUBLE=1) 30 | endif(fl_FLOATING_POINT_TYPE STREQUAL "float") 31 | -------------------------------------------------------------------------------- /cmake/doxygen.cmake: -------------------------------------------------------------------------------- 1 | ############################ 2 | # Documentation Generation # 3 | ############################ 4 | # 5 | # How to generate the documentation: 6 | # 7 | # $ cd /path/to/fl 8 | # $ mkdir build 9 | # $ cd build 10 | # $ cmake .. 11 | # $ make doc_fl 12 | # 13 | # The documentation will be generated within /path/to/fl/build/doc 14 | # 15 | 16 | set(DOC_SYNC_LOCATION "" 17 | CACHE STRING "Sync location (URL or PATH where to sync the doc to.)") 18 | 19 | set(TARGET_FAILED_SCRIPT_TEMPLATE 20 | ${CMAKE_CURRENT_SOURCE_DIR}/cmake/target_failed.cmake.in) 21 | 22 | set(TARGET_FAILED_SCRIPT 23 | ${CMAKE_CURRENT_BINARY_DIR}/cmake/target_failed.cmake) 24 | 25 | set(VERSION_SCRIPT 26 | ${CMAKE_CURRENT_SOURCE_DIR}/cmake/version.cmake) 27 | 28 | if(DOXYGEN_FOUND) 29 | execute_process(COMMAND "${DOXYGEN_EXECUTABLE}" "--version" 30 | OUTPUT_VARIABLE DOXYGEN_VERSION 31 | OUTPUT_STRIP_TRAILING_WHITESPACE) 32 | 33 | if(DOXYGEN_VERSION VERSION_LESS MIN_DOXYGEN_VERSION) 34 | set(DOXYGEN_WARN_MSG_OLD "Doxygen version is too old!") 35 | set(DOXYGEN_WARN_MSG_FOUND "Found Doxygen ${DOXYGEN_VERSION}.") 36 | set(DOXYGEN_WARN_MSG_REQ "Required is at least ${MIN_DOXYGEN_VERSION}") 37 | 38 | set(DOXYGEN_WARN_MSG "\n${DOXYGEN_WARN_MSG_OLD}\n") 39 | set(DOXYGEN_WARN_MSG "${DOXYGEN_WARN_MSG} (${DOXYGEN_WARN_MSG_FOUND}") 40 | set(DOXYGEN_WARN_MSG "${DOXYGEN_WARN_MSG} ${DOXYGEN_WARN_MSG_REQ})") 41 | message(WARNING ${DOXYGEN_WARN_MSG}) 42 | 43 | set(FATAL_ERROR_MESSAGE ${DOXYGEN_WARN_MSG}) 44 | configure_file( 45 | ${TARGET_FAILED_SCRIPT_TEMPLATE} 46 | ${TARGET_FAILED_SCRIPT} @ONLY) 47 | 48 | add_custom_target(doc_fl 49 | COMMAND ${CMAKE_COMMAND} -P ${TARGET_FAILED_SCRIPT}) 50 | add_custom_target(doc_fl_and_sync 51 | COMMAND ${CMAKE_COMMAND} -P ${TARGET_FAILED_SCRIPT}) 52 | else(DOXYGEN_VERSION VERSION_LESS MIN_DOXYGEN_VERSION) 53 | # doc_fl target 54 | add_custom_target(doc_fl 55 | COMMAND ${CMAKE_COMMAND} 56 | -D PROJECT_SOURCE_DIR:string=${PROJECT_SOURCE_DIR} 57 | -D PROJECT_BINARY_DIR:string=${PROJECT_BINARY_DIR} 58 | -P ${VERSION_SCRIPT} 59 | COMMAND ${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile 60 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} 61 | COMMENT "Generating API documentation with Doxygen" VERBATIM) 62 | 63 | # doc_fl_and_sync target 64 | configure_file( 65 | ${CMAKE_CURRENT_SOURCE_DIR}/cmake/sync_doc.cmake.in 66 | ${CMAKE_CURRENT_BINARY_DIR}/cmake/sync_doc.cmake @ONLY) 67 | 68 | add_custom_target(doc_fl_and_sync 69 | COMMAND ${CMAKE_COMMAND} 70 | -D PROJECT_SOURCE_DIR:string=${PROJECT_SOURCE_DIR} 71 | -D PROJECT_BINARY_DIR:string=${PROJECT_BINARY_DIR} 72 | -P ${VERSION_SCRIPT} 73 | COMMAND ${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile 74 | COMMAND ${CMAKE_COMMAND} -P 75 | ${CMAKE_CURRENT_BINARY_DIR}/cmake/sync_doc.cmake 76 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} 77 | COMMENT "Generating API documentation with Doxygen" VERBATIM) 78 | 79 | endif(DOXYGEN_VERSION VERSION_LESS MIN_DOXYGEN_VERSION) 80 | else(DOXYGEN_FOUND) 81 | set(DOXYGEN_WARN_MSG "Doxygen not found.") 82 | message(${DOXYGEN_WARN_MSG}) 83 | 84 | set(FATAL_ERROR_MESSAGE ${DOXYGEN_WARN_MSG}) 85 | configure_file( 86 | ${TARGET_FAILED_SCRIPT_TEMPLATE} 87 | ${TARGET_FAILED_SCRIPT} @ONLY) 88 | 89 | add_custom_target(doc_fl 90 | COMMAND ${CMAKE_COMMAND} -P ${TARGET_FAILED_SCRIPT}) 91 | endif(DOXYGEN_FOUND) 92 | -------------------------------------------------------------------------------- /cmake/gtest.cmake: -------------------------------------------------------------------------------- 1 | 2 | include(ExternalProject) 3 | include(CMakeParseArguments) 4 | 5 | if(NOT ${PROJECT_NAME}_USING_CATKIN) 6 | 7 | set(gtest_LIBRARY gtest_local) 8 | set(gtest_main_LIBRARY gtest_main_local) 9 | set(${PROJECT_NAME}_TEST_LIBS ${gtest_LIBRARY} ${gtest_main_LIBRARY}) 10 | 11 | set(GTEST_FRAMEWORK gtest_framework) 12 | 13 | ExternalProject_Add( 14 | ${GTEST_FRAMEWORK} 15 | URL https://googletest.googlecode.com/files/gtest-1.6.0.zip 16 | PREFIX ${CMAKE_CURRENT_BINARY_DIR}/gtest 17 | INSTALL_COMMAND "" # do not install this library 18 | CMAKE_ARGS -Dgtest_disable_pthreads=ON -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER} 19 | ) 20 | 21 | ExternalProject_Get_Property(${GTEST_FRAMEWORK} source_dir binary_dir) 22 | 23 | set(gtest_INCLUDE_DIR ${source_dir}/include) 24 | set(gtest_LIBRARY_PATH 25 | ${binary_dir}/${CMAKE_FIND_LIBRARY_PREFIXES}gtest.a) 26 | set(gtest_main_LIBRARY_PATH 27 | ${binary_dir}/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main.a) 28 | 29 | add_library(${gtest_LIBRARY} STATIC IMPORTED GLOBAL) 30 | set_target_properties(${gtest_LIBRARY} 31 | PROPERTIES 32 | IMPORTED_LOCATION ${gtest_LIBRARY_PATH} 33 | IMPORTED_LINK_INTERFACE_LIBRARIES "${CMAKE_THREAD_LIBS_INIT}") 34 | add_dependencies(${gtest_LIBRARY} ${GTEST_FRAMEWORK}) 35 | 36 | add_library(${gtest_main_LIBRARY} STATIC IMPORTED GLOBAL) 37 | set_target_properties(${gtest_main_LIBRARY} 38 | PROPERTIES 39 | IMPORTED_LOCATION ${gtest_main_LIBRARY_PATH} 40 | IMPORTED_LINK_INTERFACE_LIBRARIES "${CMAKE_THREAD_LIBS_INIT}") 41 | add_dependencies(${gtest_main_LIBRARY} ${gtest_LIBRARY}) 42 | 43 | include_directories(${gtest_INCLUDE_DIR}) 44 | 45 | else(NOT ${PROJECT_NAME}_USING_CATKIN) 46 | 47 | set(gtest_LIBRARY gtest) 48 | set(gtest_main_LIBRARY gtest_main) 49 | set(${PROJECT_NAME}_TEST_LIBS ${gtest_LIBRARY} ${gtest_main_LIBRARY}) 50 | 51 | endif(NOT ${PROJECT_NAME}_USING_CATKIN) 52 | 53 | function(${PROJECT_NAME}_add_test) 54 | set(options) 55 | set(oneValueArgs NAME) 56 | set(multiValueArgs SOURCES LIBS) 57 | cmake_parse_arguments(${PROJECT_NAME} 58 | "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) 59 | 60 | set(TEST_NAME "${${PROJECT_NAME}_NAME}_test") 61 | 62 | if(NOT ${PROJECT_NAME}_USING_CATKIN) 63 | add_executable(${TEST_NAME} ${${PROJECT_NAME}_SOURCES}) 64 | target_link_libraries(${TEST_NAME} 65 | ${${PROJECT_NAME}_TEST_LIBS} ${${PROJECT_NAME}_LIBS}) 66 | add_test(${TEST_NAME} ${TEST_NAME}) 67 | else(NOT ${PROJECT_NAME}_USING_CATKIN) 68 | catkin_add_gtest(${TEST_NAME} ${${PROJECT_NAME}_SOURCES}) 69 | target_link_libraries(${TEST_NAME} 70 | ${${PROJECT_NAME}_TEST_LIBS} ${${PROJECT_NAME}_LIBS}) 71 | endif(NOT ${PROJECT_NAME}_USING_CATKIN) 72 | endfunction(${PROJECT_NAME}_add_test) 73 | 74 | -------------------------------------------------------------------------------- /cmake/info.cmake: -------------------------------------------------------------------------------- 1 | ############################ 2 | # Info gen. functions # 3 | ############################ 4 | # I'm sure there is a better way of doing this... 5 | execute_process( 6 | COMMAND bash -c 7 | "v=`ps -o stat= -p $PPID` 8 | [[ $v == *+* ]] || [[ $v == *s* ]] && echo YES || echo NO" 9 | OUTPUT_VARIABLE ISATTY 10 | OUTPUT_STRIP_TRAILING_WHITESPACE) 11 | 12 | if(ISATTY STREQUAL "YES") 13 | string(ASCII 27 Esc) 14 | set(COLOR_BORDER "${Esc}[35m") 15 | set(COLOR_HEADER "${Esc}[34m") 16 | set(COLOR_BOLD "${Esc}[1m") 17 | set(COLOR_CLEAR "${Esc}[m") 18 | else(ISATTY STREQUAL "YES") 19 | set(COLOR_BORDER "") 20 | set(COLOR_HEADER "") 21 | set(COLOR_BOLD "") 22 | set(COLOR_CLEAR "") 23 | endif(ISATTY STREQUAL "YES") 24 | 25 | function(info_begin) 26 | message(STATUS "${COLOR_BORDER}=================================================${COLOR_CLEAR}") 27 | endfunction(info_begin) 28 | 29 | function(info_end) 30 | message(STATUS "${COLOR_BORDER}=====${COLOR_CLEAR}") 31 | endfunction(info_end) 32 | 33 | function(info_project project_name project_version) 34 | message(STATUS "${COLOR_BORDER}== ${COLOR_CLEAR} ${COLOR_HEADER}${project_name}${COLOR_CLEAR}") 35 | message(STATUS "${COLOR_BORDER}== ${COLOR_CLEAR} Version: ${COLOR_BOLD}${project_version}${COLOR_CLEAR}") 36 | endfunction(info_project) 37 | 38 | function(info_header list_header) 39 | message(STATUS "${COLOR_BORDER}== ${COLOR_CLEAR} ") 40 | message(STATUS "${COLOR_BORDER}== ${COLOR_CLEAR} ${COLOR_BOLD}${list_header}") 41 | endfunction(info_header) 42 | 43 | function(info_item item_name item_value) 44 | message(STATUS "${COLOR_BORDER}== ${COLOR_CLEAR} - ${item_name}:${COLOR_BOLD} ${item_value}") 45 | endfunction(info_item) 46 | -------------------------------------------------------------------------------- /cmake/sync_doc.cmake.in: -------------------------------------------------------------------------------- 1 | 2 | set(DOC_SYNC_LOCATION "@DOC_SYNC_LOCATION@") 3 | 4 | if(DOC_SYNC_LOCATION) 5 | message("\n --- Sync documentation to ${DOC_SYNC_LOCATION} ... \n ") 6 | execute_process( 7 | COMMAND rsync -ruv --force --delete ${CMAKE_CURRENT_BINARY_DIR}/doc/html/ ${DOC_SYNC_LOCATION}) 8 | else(DOC_SYNC_LOCATION) 9 | message(FATAL_ERROR "Please specify the doc location via DOC_SYNC_LOCATION !") 10 | endif(DOC_SYNC_LOCATION) 11 | -------------------------------------------------------------------------------- /cmake/target_failed.cmake.in: -------------------------------------------------------------------------------- 1 | 2 | message(FATAL_ERROR "@FATAL_ERROR_MESSAGE@") 3 | -------------------------------------------------------------------------------- /cmake/version.cmake: -------------------------------------------------------------------------------- 1 | 2 | find_package(Git) 3 | 4 | execute_process(COMMAND ${GIT_EXECUTABLE} describe --tags --always 5 | WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} 6 | OUTPUT_VARIABLE PROJECT_VERSION 7 | OUTPUT_STRIP_TRAILING_WHITESPACE) 8 | 9 | # update version in documentation config 10 | configure_file( 11 | ${PROJECT_SOURCE_DIR}/doc/Doxyfile.in 12 | ${PROJECT_BINARY_DIR}/Doxyfile @ONLY) 13 | -------------------------------------------------------------------------------- /doc/bibliography.bib: -------------------------------------------------------------------------------- 1 | % This file was created with JabRef 2.7b. 2 | % Encoding: UTF-8 3 | 4 | @ARTICLE{barry2000approximation, 5 | author = {Barry, DA and Parlange, J-Y and Li, L}, 6 | title = {Approximation for the exponential integral (Theis well function)}, 7 | journal = {Journal of Hydrology}, 8 | year = {2000}, 9 | volume = {227}, 10 | pages = {287--291}, 11 | number = {1}, 12 | publisher = {Elsevier} 13 | } 14 | 15 | @ARTICLE{giles2010approximating, 16 | author = {Giles, Mike}, 17 | title = {Approximating the erfinv function}, 18 | journal = {GPU Gems}, 19 | year = {2010}, 20 | volume = {4} 21 | } 22 | 23 | @ARTICLE{matsumoto1998mersenne, 24 | author = {Matsumoto, Makoto and Nishimura, Takuji}, 25 | title = {Mersenne twister: a 623-dimensionally equidistributed uniform pseudo-random 26 | number generator}, 27 | journal = {ACM Transactions on Modeling and Computer Simulation (TOMACS)}, 28 | year = {1998}, 29 | volume = {8}, 30 | pages = {3--30}, 31 | number = {1}, 32 | publisher = {ACM} 33 | } 34 | 35 | @BOOK{press2007numerical, 36 | title = {Numerical recipes 3rd edition: The art of scientific computing}, 37 | publisher = {Cambridge university press}, 38 | year = {2007}, 39 | author = {Press, William H} 40 | } 41 | 42 | @INPROCEEDINGS{wan2000unscented, 43 | author = {Wan, Eric A and Van Der Merwe, Rudolph}, 44 | title = {The unscented Kalman filter for nonlinear estimation}, 45 | booktitle = {Adaptive Systems for Signal Processing, Communications, and Control 46 | Symposium 2000. AS-SPCC. The IEEE 2000}, 47 | year = {2000}, 48 | pages = {153--158}, 49 | organization = {IEEE} 50 | } 51 | 52 | @ARTICLE{wu2006numerical, 53 | author = {Wu, Yuanxin and Hu, Dewen and Wu, Meiping and Hu, Xiaoping}, 54 | title = {A numerical-integration perspective on Gaussian filters}, 55 | journal = {Signal Processing, IEEE Transactions on}, 56 | year = {2006}, 57 | volume = {54}, 58 | pages = {2910--2921}, 59 | number = {8}, 60 | publisher = {IEEE} 61 | } 62 | 63 | -------------------------------------------------------------------------------- /doc/filter_concept.dox: -------------------------------------------------------------------------------- 1 | namespace fl 2 | { 3 | 4 | /** 5 | \page page_filter_concept Filter Concept 6 | 7 | \tableofcontents 8 | 9 | All filter share a 10 | common static \c concept or \c interface represented by the 11 | 12 | @code 13 | class FilterInterface; 14 | @endcode 15 | 16 | The \c FilterAlgorithm type will be discussed in deeper detains in the next section. 17 | 18 | Given any algorithm implementation \c FilterAlgorithm, the class 19 | \c FilterInterface is guaranteed to provide you access to the following types 20 | 21 | Types | Description 22 | --------------------------------------------------- | ---------------------------------------------------------------------- 23 | FilterInterface::State | State type defining the state space 24 | FilterInterface::Input | Process control input type used within the system dynamics 25 | FilterInterface::Observation | Measurement type used within the measurement model to update the state 26 | FilterInterface::StateDistribution | Distribution type over the state which always implements the ff::Moments interface providing the first and the second moments (mean and covariance) 27 | 28 | The interface is depicted in the UML diagram blow 29 | 30 | ![FilterInterface Concept Class](filter_interface.svg) 31 | 32 | During the filtering process the client will invoke the FilterInterface::predict 33 | and the FilterInterface::update methods interchangeably as demonstrated in the 34 | pseudocode listings below 35 | 36 | \code{python} 37 | FilterInterface filter = FilterAlgorithm(/* filter arguments ... */); 38 | 39 | Time t = Time::now(); 40 | while (running()) 41 | { 42 | Observation y = wait_and_get_observation(); 43 | 44 | filter.predict(Time::now() - t, ...); 45 | filter.update(y, ...); 46 | 47 | t = Time::now(); 48 | } 49 | \endcode 50 | 51 | or call \c predict several times until a measurement is available in order to invoke 52 | \c update 53 | 54 | \code{python} 55 | FilterInterface filter = FilterAlgorithm(/* filter arguments ... */); 56 | 57 | Time t = Time::now(); 58 | while (running()) 59 | { 60 | while (!new_observation_available()) 61 | { 62 | filter.predict(Time::now() - t, ...); 63 | } 64 | 65 | Observation y = get_observation(); 66 | 67 | filter.update(y, ...); 68 | 69 | t = Time::now(); 70 | } 71 | \endcode 72 | 73 | Alternatively, the FilterInterface::predict_and_update can be called repeatedly 74 | instead 75 | 76 | \code{python} 77 | FilterInterface filter = FilterAlgorithm(/* filter arguments ... */); 78 | 79 | Time t = Time::now(); 80 | while (running()) 81 | { 82 | Observation y = wait_and_get_observation(); 83 | filter.predict_and_filter(Time::now() - t, ..., y, ...); 84 | 85 | t = Time::now(); 86 | } 87 | \endcode 88 | 89 | Depending on the \c FilterAlgorithm implementation, the combined 90 | function \c predict_and_update may be fast. In this case the algorithm is able 91 | to exploit the all at once structure. 92 | 93 | Next: \ref page_constructing_filters 94 | 95 | */ 96 | 97 | } 98 | -------------------------------------------------------------------------------- /doc/getting_started.dox: -------------------------------------------------------------------------------- 1 | 2 | namespace fl 3 | { 4 | 5 | /** 6 | \page page_getting_started Getting Started 7 | 8 | \tableofcontents 9 | 10 | FL is a pure template based library. The general usage is to run a filter of 11 | choice with predefined system dynamics and state space. 12 | 13 | Before diving 14 | into the construction and usage of filters, it is important to understand 15 | the basic concept and interface of the filtering algorithms in this library. 16 | 17 | -# \subpage page_filter_concept 18 | -# \subpage page_constructing_filters 19 | -# \subpage page_using_filters 20 | */ 21 | 22 | } -------------------------------------------------------------------------------- /doc/howto.dox: -------------------------------------------------------------------------------- 1 | 2 | namespace fl 3 | { 4 | 5 | /** 6 | \page page_howto How to ... 7 | - \subpage page_implement_filter 8 | - \subpage page_implement_model 9 | - \subpage page_implement_distribution 10 | - \subpage page_implement_transform 11 | */ 12 | 13 | /** \page page_implement_model Write your own model 14 | */ 15 | /** \page page_implement_distribution Write your own distribution 16 | */ 17 | /** \page page_implement_transform Write your own transform policy 18 | */ 19 | 20 | 21 | } 22 | -------------------------------------------------------------------------------- /doc/images/bayes.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/filtering-library/fl/9117e240361b43bdead8b506b755235fb6d5e699/doc/images/bayes.jpg -------------------------------------------------------------------------------- /doc/mainpage.dox: -------------------------------------------------------------------------------- 1 | 2 | namespace fl 3 | { 4 | 5 | /** 6 | 7 | \mainpage notitle 8 | 9 | \tableofcontents 10 | 11 | \section about_fl About FL (draft) 12 | 13 | FL is general purpose C++ ([C++11](http://en.wikipedia.org/wiki/C%2B%2B11) 14 | standard) Bayesian filtering framework library with real-time support for 15 | linear and non-linear systems. Currently available filters are (just an 16 | example list) 17 | 18 | 19 | - General Particle Filter 20 | - Coordinate Particle Filter 21 | - KalmanFilter for purely linear systems 22 | - Sigma Point Kalman Filter for linear and non-linear systems 23 | 24 | 25 | The library is being developed as an extensible framework with low application 26 | restrictions. 27 | 28 | The employed underlying concepts are aimed to use this library in 29 | regular offline mode applications as well as in real-time systems. As a result 30 | of the real-time capability, the user has the option to take advantage of 31 | vectorization optimizations (SEE2, SSE3 or SSE4, AltiVec, ARM NEON) and zero 32 | runtime heap allocation all together with minimum effort. 33 | 34 | \section dependecies Dependencies and Compiling 35 | 36 | FL doesn't require pre-complication nor any library linking. The entire code 37 | base is generic and based on templates. 38 | 39 | The library dependencies are 40 | 41 | - Eigen3 ([Eigen 3.1.2 or higher](http://eigen.tuxfamily.org/)) 42 | 43 | To be able to compile your code with FL you need to enable the C++11 or C++0x 44 | standard for your compiler. With GCC you may add the definition 45 | 46 | -std=c++0x OR -std=c++11 47 | 48 | \subsection tested_eigen Tested Eigen3 49 | 50 | - Eigen 3.1.2 51 | - Eigen 3.2.0 52 | 53 | \subsection tested_compilers Tested Compilers 54 | 55 | - GCC 4.6 (-std=c++0x) 56 | - GCC 4.8 (-std=c++11) 57 | 58 | \subsection tested_os Tested OS 59 | 60 | - Ubuntu 12.04 LTS (Precise (AMD64)) 61 | - Ubuntu 14.04 LTS (Trusty Tahr (AMD64)) 62 | */ 63 | 64 | } 65 | -------------------------------------------------------------------------------- /doc/modules.dox: -------------------------------------------------------------------------------- 1 | 2 | namespace fl 3 | { 4 | 5 | /** 6 | \defgroup filters Filter Algorithms 7 | \defgroup distributions Distributions 8 | \defgroup sensors Observation Models 9 | \defgroup transitions Process Models 10 | \defgroup numeric_integration Numeric Integration 11 | 12 | \defgroup traits Traits 13 | \defgroup meta Meta 14 | \defgroup math Math 15 | \defgroup types Types 16 | \defgroup util Util 17 | \defgroup exceptions Exceptions 18 | */ 19 | 20 | /** 21 | * \defgroup distribution_interfaces Interfaces 22 | * \ingroup distributions 23 | */ 24 | 25 | /** 26 | * \defgroup special_functions Special Functions 27 | * \ingroup math 28 | */ 29 | 30 | /** 31 | * \defgroup general_functions General Functions 32 | * \ingroup math 33 | */ 34 | 35 | /** 36 | * \defgroup linear_algebra Linear Algebra 37 | * \ingroup math 38 | */ 39 | 40 | } 41 | -------------------------------------------------------------------------------- /doc/theme/customization.css: -------------------------------------------------------------------------------- 1 | div.contents 2 | { 3 | max-width:55em; 4 | } 5 | 6 | p, dl.warning, dl.attention, dl.note 7 | { 8 | text-align:justify; 9 | } 10 | 11 | li { 12 | text-align:justify; 13 | } 14 | 15 | div.fragment { 16 | display: table; 17 | } 18 | 19 | table.doxtable, table.example th, table.manual th, table.manual-vl th { 20 | padding: 0.5em 0.5em 0.5em 0.5em; 21 | text-align: left; 22 | padding-right: 1em; 23 | color: #555555; 24 | background-color: #F4F4E5; 25 | 26 | background-image: -webkit-gradient(linear,center top,center bottom,from(#FFFFFF), color-stop(0.3,#FFFFFF), color-stop(0.30,#FFFFFF), color-stop(0.98,#F4F4E5), to(#ECECDE)); 27 | background-image: -moz-linear-gradient(center top, #FFFFFF 0%, #FFFFFF 30%, #F4F4E5 98%, #ECECDE); 28 | filter: progid:DXImageTransform.Microsoft.gradient(startColorstr='#FFFFFF', endColorstr='#F4F4E5'); 29 | } 30 | 31 | table.doxtable td, table.example td, table.manual td, table.manual-vl td { 32 | vertical-align:top; 33 | border-width: 1px; 34 | border-color: #cccccc; 35 | } 36 | 37 | table.tparams td 38 | { 39 | vertical-align:top; 40 | } 41 | 42 | 43 | h2 { 44 | margin-top:2em; 45 | border-style: none none solid none; 46 | border-width: 1px; 47 | border-color: #cccccc; 48 | } 49 | 50 | div.toc { 51 | width:40%; 52 | margin-right:0; 53 | } 54 | 55 | /* @group Code Colorization */ 56 | 57 | span.keyword { 58 | color: #a71d5d; 59 | 60 | } 61 | 62 | span.keywordtype { 63 | color: #b58900 64 | } 65 | 66 | span.keywordflow { 67 | color: #e08000 68 | } 69 | 70 | span.comment { 71 | color: #969896 72 | } 73 | 74 | span.preprocessor { 75 | color: #6c71c4 76 | } 77 | 78 | span.stringliteral { 79 | color: #2aa198 80 | } 81 | 82 | span.charliteral { 83 | color: #008080 84 | } 85 | 86 | span.concept { 87 | color: #795da3; 88 | } 89 | 90 | span.traits { 91 | font-weight: bold; 92 | } 93 | 94 | span.doccommand { 95 | color: #2aa198; 96 | } 97 | 98 | span.function { 99 | color: #0086b3; 100 | } 101 | 102 | span.staticfunction { 103 | color: #800000; 104 | } 105 | 106 | div.fragment { 107 | color: #333; 108 | font-family: Consolas, 'Liberation Mono', Menlo, Courier, monospace; 109 | font-size: 12px; 110 | font-style: normal; 111 | font-variant: normal; 112 | font-weight: normal; 113 | line-height: 16.7999992370605px; 114 | 115 | width: 100%; 116 | padding: 0 0 0 0; 117 | margin: 0 0 0 0; 118 | background-color: #FFFFFF; 119 | border: 1px solid #C4CFE5; 120 | } 121 | 122 | div.line{ 123 | color: #333; 124 | font-family: Consolas, 'Liberation Mono', Menlo, Courier, monospace; 125 | font-size: 12px; 126 | font-style: normal; 127 | font-variant: normal; 128 | font-weight: normal; 129 | line-height: 16.7999992370605px; 130 | } 131 | 132 | 133 | /* @end */ 134 | -------------------------------------------------------------------------------- /doc/theme/navtree_expand_selected.js: -------------------------------------------------------------------------------- 1 | // Overloaded to remove links to sections/subsections 2 | function getNode(o, po) 3 | { 4 | po.childrenVisited = true; 5 | var l = po.childrenData.length-1; 6 | for (var i in po.childrenData) { 7 | var nodeData = po.childrenData[i]; 8 | if((!nodeData[1]) || (nodeData[1].indexOf('#')==-1)) // <- we added this line 9 | po.children[i] = newNode(o, po, nodeData[0], nodeData[1], nodeData[2], i==l); 10 | } 11 | } 12 | 13 | function selectAndHighlight(hash,n) 14 | { 15 | var a; 16 | if (hash) { 17 | var link=stripPath($(location).attr('pathname'))+':'+hash.substring(1); 18 | a=$('.item a[class$="'+link+'"]'); 19 | } 20 | if (a && a.length) { 21 | a.parent().parent().addClass('selected'); 22 | a.parent().parent().attr('id','selected'); 23 | highlightAnchor(); 24 | } else if (n) { 25 | $(n.itemDiv).addClass('selected'); 26 | $(n.itemDiv).attr('id','selected'); 27 | } 28 | if ($('#nav-tree-contents .item:first').hasClass('selected')) { 29 | $('#nav-sync').css('top','30px'); 30 | } else { 31 | $('#nav-sync').css('top','5px'); 32 | } 33 | expandNode(new Object(), n, true, true); 34 | showRoot(); 35 | }; 36 | 37 | 38 | // return false if the the node has no children at all, or has only section/subsection children 39 | function checkChildrenData(node) { 40 | if (!(typeof(node.childrenData)==='string')) { 41 | for (var i in node.childrenData) { 42 | var url = node.childrenData[i][1]; 43 | if(url.indexOf("#")==-1) 44 | return true; 45 | } 46 | return false; 47 | } 48 | return (node.childrenData); 49 | }; 50 | 51 | // Modified to: 52 | // - remove the section/subsection children 53 | function createIndent(o,domNode,node,level) 54 | { 55 | var level=-1; 56 | var n = node; 57 | while (n.parentNode) { level++; n=n.parentNode; } 58 | var imgNode = document.createElement("img"); 59 | imgNode.style.paddingLeft=(16*(level)).toString()+'px'; 60 | imgNode.width = 16; 61 | imgNode.height = 22; 62 | imgNode.border = 0; 63 | if (checkChildrenData(node)) { // <- we modified this line to use checkChildrenData(node) instead of node.childrenData 64 | node.plus_img = imgNode; 65 | node.expandToggle = document.createElement("a"); 66 | node.expandToggle.href = "javascript:void(0)"; 67 | node.expandToggle.onclick = function() { 68 | if (node.expanded) { 69 | $(node.getChildrenUL()).slideUp("fast"); 70 | node.plus_img.src = node.relpath+"ftv2pnode.png"; 71 | node.expanded = false; 72 | } else { 73 | expandNode(o, node, false, false); 74 | } 75 | } 76 | node.expandToggle.appendChild(imgNode); 77 | domNode.appendChild(node.expandToggle); 78 | imgNode.src = node.relpath+"ftv2pnode.png"; 79 | } else { 80 | imgNode.src = node.relpath+"ftv2node.png"; 81 | domNode.appendChild(imgNode); 82 | } 83 | }; 84 | -------------------------------------------------------------------------------- /include/fl/distribution/cauchy_distribution.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file cauchy_distribution.hpp 16 | * \date August 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | namespace fl 28 | { 29 | 30 | /** 31 | * \ingroup distributions 32 | * 33 | * \brief CauchyDistribution represents a multivariate cauchy distribution. It 34 | * is a special case of student's t-distribution and is equal to 35 | * \f$t_1(\mu, \Sigma)\f$. 36 | */ 37 | template 38 | class CauchyDistribution 39 | : public TDistribution 40 | { 41 | public: 42 | /** 43 | * Creates a dynamic or fixed size multivariate cauchy distribution 44 | * 45 | * \param dimension Dimension of the distribution. The default is defined by 46 | * the dimension of the variable type \em Vector. If the 47 | * size of the Variate at compile time is fixed, this will 48 | * be adapted. For dynamic-sized Variable the dimension is 49 | * initialized to 0. 50 | */ 51 | explicit CauchyDistribution(int dim = DimensionOf()) 52 | : TDistribution(Real(1), dim) 53 | { } 54 | }; 55 | 56 | } 57 | -------------------------------------------------------------------------------- /include/fl/distribution/exponential_distribution.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file exponential_distribution.hpp 16 | * \date May 2014 17 | * \author Manuel Wuthrich (manuel.wuthrich@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | namespace fl 31 | { 32 | 33 | /// \todo MISSING DOC. MISSING UTESTS 34 | 35 | /** 36 | * \ingroup distributions 37 | */ 38 | class ExponentialDistribution: 39 | public Evaluation, 40 | public StandardGaussianMapping 41 | { 42 | 43 | public: 44 | ExponentialDistribution(Real lambda, 45 | Real min = 0, 46 | Real max = std::numeric_limits::infinity()): 47 | lambda_(lambda), 48 | min_(min), 49 | max_(max) 50 | { 51 | exp_lambda_min_ = std::exp(-lambda_*min); 52 | exp_lambda_max_ = std::exp(-lambda_*max); 53 | } 54 | 55 | virtual ~ExponentialDistribution() noexcept { } 56 | 57 | virtual Real probability(const Real& input) const 58 | { 59 | if(input < min_ || input > max_) 60 | return 0; 61 | 62 | return lambda_ * std::exp(-lambda_ * input) / 63 | (exp_lambda_min_ - exp_lambda_max_); 64 | } 65 | 66 | virtual Real log_probability(const Real& input) const 67 | { 68 | return std::log(probability(input)); 69 | } 70 | 71 | virtual Real map_standard_normal(const Real& gaussian_sample) const 72 | { 73 | // map from a gaussian to a uniform distribution 74 | Real uniform_sample = 0.5 * 75 | (1.0 + std::erf(gaussian_sample / std::sqrt(2.0))); 76 | // map from a uniform to an exponential distribution 77 | return -std::log(exp_lambda_min_ - (exp_lambda_min_ - exp_lambda_max_) 78 | * uniform_sample) / lambda_; 79 | } 80 | 81 | virtual Real map_standard_normal(const Real& gaussian_sample, 82 | const Real& max) const 83 | { 84 | Real exp_lambda_max = std::exp(-lambda_*max); 85 | 86 | // map from a gaussian to a uniform distribution 87 | Real uniform_sample = 0.5 * 88 | (1.0 + std::erf(gaussian_sample / std::sqrt(2.0))); 89 | // map from a uniform to an exponential distribution 90 | return -std::log(exp_lambda_min_ - (exp_lambda_min_ - exp_lambda_max) 91 | * uniform_sample) / lambda_; 92 | } 93 | 94 | private: 95 | Real lambda_; 96 | Real min_; 97 | Real max_; 98 | Real exp_lambda_min_; 99 | Real exp_lambda_max_; 100 | }; 101 | 102 | } 103 | -------------------------------------------------------------------------------- /include/fl/distribution/interface/approximate_moments.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file approximate_moments.hpp 16 | * \date May 2014 17 | * \author Manuel Wuthrich (manuel.wuthrich@gmail.com) 18 | * \author Jan Issac (jan.issac@gmail.com) 19 | */ 20 | 21 | #pragma once 22 | 23 | 24 | #include 25 | #include 26 | 27 | namespace fl 28 | { 29 | 30 | // Forward declaration 31 | template class ApproximateMoments; 32 | 33 | /** 34 | * \ingroup distribution_interfaces 35 | * 36 | * \brief Represents the interface providing the first two central moments 37 | * 38 | * \tparam Variate Random variable type. This is equivalent to the 39 | * first moment type. 40 | * 41 | * The ApproximateMoments interface provides access to a numerical approximation 42 | * of the first moments of a distribution. 43 | * 44 | */ 45 | template 46 | class ApproximateMoments 47 | { 48 | public: 49 | /** 50 | * \brief Variate Random variable type. This is equivalent to the first 51 | * moment type. 52 | */ 53 | typedef Variate_ Variate; 54 | 55 | /** 56 | * \brief Second central moment type (e.g. Variance or the Covariance) 57 | */ 58 | typedef SecondMoment_ SecondMoment; 59 | 60 | 61 | /** 62 | * \brief Overridable default destructor 63 | */ 64 | virtual ~ApproximateMoments() noexcept { } 65 | 66 | /** 67 | * \brief Returns the first moment approximation, the mean 68 | * 69 | * \f$ \mu_{approx} \approx \sum\limits_i x_i p(x_i)\f$ 70 | */ 71 | virtual const Variate& approximate_mean() const = 0; 72 | 73 | /** 74 | * \brief Returns the second centeral moment, the covariance 75 | * 76 | * \f$ \Sigma_{approx} \approx 77 | * \sum\limits_i (x_i - \mu)(x_i - \mu)^T \f$ 78 | */ 79 | virtual const SecondMoment& approximate_covariance() const = 0; 80 | }; 81 | 82 | /** 83 | * \ingroup distribution_interfaces 84 | */ 85 | template 86 | class ApproximateMoments 87 | : public ApproximateMoments::Type> 88 | { 89 | public: 90 | /** 91 | * \brief Overridable default destructor 92 | */ 93 | virtual ~ApproximateMoments() noexcept { } 94 | }; 95 | 96 | /** 97 | * \ingroup distribution_interfaces 98 | */ 99 | template <> 100 | class ApproximateMoments 101 | : public ApproximateMoments 102 | { 103 | public: 104 | /** 105 | * \brief Overridable default destructor 106 | */ 107 | virtual ~ApproximateMoments() noexcept { } 108 | }; 109 | 110 | } 111 | -------------------------------------------------------------------------------- /include/fl/distribution/interface/evaluation.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file evaluation.hpp 16 | * \date May 2014 17 | * \author Manuel Wuthrich (manuel.wuthrich@gmail.com) 18 | * \author Jan Issac (jan.issac@gmail.com) 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | namespace fl 29 | { 30 | 31 | /** 32 | * \ingroup distribution_interfaces 33 | * 34 | * \brief Distribution evaulation interface 35 | * 36 | * \tparam Variate Random variable type 37 | * \tparam Scalar Probability & log scalar type 38 | * 39 | * Evaluation provides the interface to determine the probability of the 40 | * underlying distribution at a given sample. Evaluation is a subset of 41 | * unnormalized distributions. Normalized in this context means 42 | * \f[ \int\limits_{-\infty}^{\infty} p(x) dx = 1. \f] 43 | */ 44 | template 45 | class Evaluation: 46 | public UnnormalizedEvaluation 47 | { 48 | public: 49 | /** 50 | * \brief Overridable default destructor 51 | */ 52 | virtual ~Evaluation() noexcept { } 53 | 54 | /** 55 | * \brief Determines the probability for the specified variate. 56 | * 57 | * \param variate Sample \f$x\f$ to evaluate 58 | * 59 | * \return \f$p(x)\f$ 60 | */ 61 | virtual Real probability(const Variate& variate) const 62 | { 63 | return std::exp(log_probability(variate)); 64 | } 65 | 66 | /** 67 | * \brief Determines the probability log for the specified variate. 68 | * 69 | * \param variate Sample \f$x\f$ to evaluate 70 | * 71 | * \return \f$\ln(p(x))\f$ 72 | */ 73 | virtual Real log_probability(const Variate& variate) const = 0; 74 | 75 | Real log_unnormalized_probability(const Variate& variate) const override 76 | { 77 | return log_probability(variate); 78 | } 79 | }; 80 | 81 | } 82 | -------------------------------------------------------------------------------- /include/fl/distribution/interface/moments.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file moments.hpp 16 | * \date May 2014 17 | * \author Manuel Wuthrich (manuel.wuthrich@gmail.com) 18 | * \author Jan Issac (jan.issac@gmail.com) 19 | */ 20 | 21 | #pragma once 22 | 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | namespace fl 29 | { 30 | 31 | #ifndef GENERATING_DOCUMENTATION 32 | // Forward declaration 33 | template class Moments; 34 | #endif 35 | 36 | /** 37 | * \ingroup distribution_interfaces 38 | * 39 | * \brief Represents the interface providing the first two moments 40 | * 41 | * \tparam Variate Random variable type. This is equivalent to the first 42 | * moment type. 43 | * \tparam SecondMoment Second moment type. The second moment is either 44 | * the second uncentered moment \f$Var(X) + X^2\f$ or 45 | * simply the second central moment, the variance or 46 | * covariance \f$Var(X) = Cov(X, X)\f$. Both have the 47 | * same type \c SecondMoment. 48 | * 49 | * 50 | * The Moments interface provides access to the exact first moments of 51 | * a distribution. The moments represent a subset of the approximate moments. 52 | */ 53 | template 54 | #ifndef GENERATING_DOCUMENTATION 55 | class Moments 56 | #else 57 | class Moments 58 | #endif 59 | : public ApproximateMoments 60 | { 61 | public: 62 | /** 63 | * \brief Variate Random variable type. This is equivalent to the first 64 | * moment type. 65 | */ 66 | typedef Variate_ Variate; 67 | 68 | /** 69 | * \brief Second central moment type (e.g. Variance or the Covariance) 70 | */ 71 | typedef SecondMoment_ SecondMoment; 72 | 73 | /** 74 | * \brief Overridable default destructor 75 | */ 76 | virtual ~Moments() noexcept { } 77 | 78 | /** 79 | * \brief Returns the first moment of the underlying distribution, the mean 80 | * 81 | * \f$ \mu = \sum\limits_i x_i p(x_i)\f$ 82 | */ 83 | virtual const Variate& mean() const = 0; 84 | 85 | /** 86 | * \brief Returns the second centered moment of the underlying distribution, 87 | * the covariance 88 | * 89 | * \f$ \Sigma = 90 | * \sum\limits_i (x_i - \mu)(x_i - \mu)^T \f$ 91 | */ 92 | virtual const SecondMoment& covariance() const = 0; 93 | 94 | const Variate& approximate_mean() const override 95 | { 96 | return mean(); 97 | } 98 | 99 | const SecondMoment& approximate_covariance() const override 100 | { 101 | return covariance(); 102 | } 103 | }; 104 | 105 | /** 106 | * \ingroup distribution_interfaces 107 | * 108 | */ 109 | template 110 | class Moments 111 | : public Moments::Type> 112 | { 113 | public: 114 | /** 115 | * \brief Overridable default destructor 116 | */ 117 | virtual ~Moments() noexcept { } 118 | }; 119 | 120 | /** 121 | * \ingroup distribution_interfaces 122 | * 123 | */ 124 | template < > 125 | class Moments 126 | : public Moments 127 | { 128 | public: 129 | /** 130 | * \brief Overridable default destructor 131 | */ 132 | virtual ~Moments() noexcept { } 133 | }; 134 | 135 | } 136 | -------------------------------------------------------------------------------- /include/fl/distribution/interface/sampling.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file sampling.hpp 16 | * \date May 2014 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | * \author Manuel Wuthrich (manuel.wuthrich@gmail.com) 19 | */ 20 | 21 | #pragma once 22 | 23 | 24 | namespace fl 25 | { 26 | 27 | /** 28 | * \ingroup distribution_interfaces 29 | * 30 | * \brief Distribution sampling interface 31 | * 32 | * \tparam Variate Variate type of the random variable 33 | */ 34 | template 35 | class Sampling 36 | { 37 | public: 38 | /** 39 | * \brief Overridable default destructor 40 | */ 41 | virtual ~Sampling() noexcept { } 42 | 43 | /** 44 | * \return A random sample of the underlying distribution \f[x \sim p(x)\f] 45 | */ 46 | virtual Variate sample() const = 0; 47 | }; 48 | 49 | } 50 | -------------------------------------------------------------------------------- /include/fl/distribution/interface/unnormalized_evaluation.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file unnormalized_evaluation.hpp 16 | * \date May 2014 17 | * \author Manuel Wuthrich (manuel.wuthrich@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | #include 23 | #include 24 | 25 | namespace fl 26 | { 27 | 28 | /** 29 | * \ingroup distribution_interfaces 30 | * 31 | * \brief Represents the part of a distribution interface 32 | * which evaluates the unnormalized probability \f$\tilde{p}(x)\f$ of a sample 33 | * \f$x\f$ according to the implemented distribution. Unnormalized in this 34 | * context means the integral 35 | * 36 | * \f[ \int\limits_{-\infty}^{\infty} \tilde{p}(x) dx \in (-\infty; \infty) \f] 37 | * 38 | * does not necessarliy integrate to 1. 39 | */ 40 | template 41 | class UnnormalizedEvaluation 42 | { 43 | public: 44 | virtual ~UnnormalizedEvaluation() noexcept { } 45 | 46 | /** 47 | * \brief Determines the normalized or unnormalized probability for the 48 | * specified variate. 49 | * 50 | * \param variate The sample \f$x\f$ to evaluate 51 | * 52 | * \return \f$\tilde{p}(x)\f$ 53 | */ 54 | virtual Real unnormalized_probability(const Variate& variate) const 55 | { 56 | return std::exp(log_unnormalized_probability(variate)); 57 | } 58 | 59 | /** 60 | * \brief Returns log of the unnormalized probability of the specified 61 | * variate 62 | * 63 | * \param variate The sample \f$x\f$ to evaluate 64 | * 65 | * \return \f$\ln(\tilde{p}(x))\f$ 66 | */ 67 | virtual Real log_unnormalized_probability(const Variate& variate) const = 0; 68 | }; 69 | 70 | } 71 | -------------------------------------------------------------------------------- /include/fl/distribution/joint_distribution.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file joint_distribution.hpp 16 | * \date Febuary 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | 26 | -------------------------------------------------------------------------------- /include/fl/distribution/truncated_gaussian.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file truncated_gaussian.hpp 16 | * \date May 2014 17 | * \author Manuel Wuthrich (manuel.wuthrich@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | namespace fl 31 | { 32 | 33 | /// \todo MISSING DOC. MISSING UTESTS 34 | /** 35 | * \ingroup distributions 36 | */ 37 | class TruncatedGaussian 38 | : public Evaluation, 39 | public StandardGaussianMapping 40 | { 41 | public: 42 | TruncatedGaussian(Real mean = 0.0, 43 | Real sigma = 1.0, 44 | Real min = -std::numeric_limits::infinity(), 45 | Real max = std::numeric_limits::infinity()) 46 | : mean_(mean), 47 | sigma_(sigma), 48 | min_(min), 49 | max_(max) 50 | { 51 | ComputeAuxiliaryParameters(); 52 | } 53 | 54 | virtual ~TruncatedGaussian() noexcept { } 55 | 56 | virtual void parameters(Real mean, Real sigma, Real min, Real max) 57 | { 58 | mean_ = mean; 59 | sigma_ = sigma; 60 | min_ = min; 61 | max_ = max; 62 | 63 | ComputeAuxiliaryParameters(); 64 | } 65 | 66 | virtual Real probability(const Real& input) const 67 | { 68 | if(input < min_ || input > max_) 69 | return 0; 70 | 71 | return normalization_factor_ * 72 | std::exp(-0.5 * std::pow((input-mean_)/sigma_, 2)); 73 | } 74 | 75 | virtual Real log_probability(const Real& input) const 76 | { 77 | return std::log(probability(input)); 78 | } 79 | 80 | virtual Real map_standard_normal(const Real& gaussian_sample) const 81 | { 82 | // map from a gaussian to a uniform distribution 83 | Real standard_uniform_sample = 0.5 * 84 | (1.0 + std::erf(gaussian_sample / std::sqrt(2.0))); 85 | // map onto truncated uniform distribution 86 | Real truncated_uniform_sample = cumulative_min_ + 87 | standard_uniform_sample * (cumulative_max_ - cumulative_min_); 88 | // map onto truncated gaussian 89 | 90 | return mean_ + sigma_ * std::sqrt(2.0) * 91 | fl::erfinv(2.0 * truncated_uniform_sample - 1.0); 92 | } 93 | 94 | private: 95 | virtual void ComputeAuxiliaryParameters() 96 | { 97 | cumulative_min_ = 0.5 + 98 | 0.5 * std::erf( (min_-mean_) / (sigma_*std::sqrt(2)) ); 99 | cumulative_max_ = 0.5 + 100 | 0.5 * std::erf( (max_-mean_) / (sigma_*std::sqrt(2)) ); 101 | 102 | normalization_factor_ = 1.0 / 103 | (sigma_ * (cumulative_max_-cumulative_min_) * std::sqrt(2.0*M_PI)); 104 | } 105 | 106 | private: 107 | Real mean_; 108 | Real sigma_; 109 | Real min_; 110 | Real max_; 111 | 112 | Real cumulative_min_; 113 | Real cumulative_max_; 114 | Real normalization_factor_; 115 | }; 116 | 117 | } 118 | -------------------------------------------------------------------------------- /include/fl/distribution/uniform_distribution.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file uniform_distribution.hpp 16 | * \date May 2014 17 | * \author Manuel Wuthrich (manuel.wuthrich@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | namespace fl 31 | { 32 | 33 | /// \todo MISSING DOC. MISSING UTESTS 34 | 35 | /** 36 | * \ingroup distributions 37 | */ 38 | class UniformDistribution 39 | : public Evaluation, 40 | public StandardGaussianMapping 41 | { 42 | private: 43 | typedef StandardGaussianMapping StdGaussianMappingBase; 44 | 45 | public: 46 | typedef Vector1d Variate; 47 | 48 | /** 49 | * \brief Represents the StandardGaussianMapping standard variate type which 50 | * is of the same dimension as the \c TDistribution \c Variate. The 51 | * StandardVariate type is used to sample from a standard normal 52 | * Gaussian and map it to this \c TDistribution 53 | */ 54 | typedef typename StdGaussianMappingBase::StandardVariate StandardVariate; 55 | 56 | public: 57 | UniformDistribution() 58 | : min_(0.), max_(1.) 59 | { 60 | init(); 61 | } 62 | 63 | UniformDistribution(Real min, Real max) 64 | : min_(min), max_(max) 65 | { 66 | init(); 67 | } 68 | 69 | virtual ~UniformDistribution() noexcept { } 70 | 71 | Real probability(const Variate& x) const override 72 | { 73 | assert(x.size() == 1); 74 | 75 | if(x(0) < min_ || x(0) > max_) return 0; 76 | 77 | return density_; 78 | } 79 | 80 | Real log_probability(const Variate& x) const override 81 | { 82 | assert(x.size() == 1); 83 | 84 | if(x(0) < min_ || x(0) > max_) 85 | { 86 | return -std::numeric_limits::infinity(); 87 | } 88 | 89 | return log_density_; 90 | } 91 | 92 | Variate map_standard_normal(const StandardVariate& sample) const override 93 | { 94 | assert(sample.size() == 1); 95 | 96 | // map from a gaussian to a uniform distribution 97 | Real standard_uniform_sample = fl::normal_to_uniform(sample(0)); 98 | 99 | Variate v; 100 | v(0) = mean_ + (standard_uniform_sample - 0.5) * delta_; 101 | 102 | return v; 103 | } 104 | 105 | private: 106 | void init() 107 | { 108 | delta_ = max_ - min_; 109 | density_ = 1.0 / delta_; 110 | mean_ = (max_ + min_) / 2.0; 111 | log_density_ = std::log(density_); 112 | } 113 | 114 | Real min_; 115 | Real max_; 116 | Real delta_; 117 | Real mean_; 118 | Real density_; 119 | Real log_density_; 120 | }; 121 | 122 | } 123 | -------------------------------------------------------------------------------- /include/fl/filter/filter_interface.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file filter_interface.hpp 16 | * \date October 2014 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | 26 | namespace fl 27 | { 28 | 29 | /** 30 | * \interface FilterInterface 31 | * 32 | * \brief Common filter interface 33 | * 34 | * 35 | * FilterInterface represents the common interface of all filters. Each filter 36 | * must implement the two filter steps, predict/propagate and update. 37 | * The filtered state, observation and the state distribution are subject to the 38 | * underlying filtering algorithm and therefore may differ from one to another. 39 | * This interface, provides these types of the actual algorithm, such as the 40 | * the State, Obsrv (Observation), and Belief types. Therefore, the 41 | * traits of each filter implememntation must provide the following types 42 | * 43 | * Required Types | Description | Requirements 44 | * -------------- | ---------------------------------- | --------------- 45 | * \c State | Used State type | - 46 | * \c Input | Process control input type | - 47 | * \c Obsrv | Used observation type | - 48 | * \c Belief | Distribution type over the state | implements fl::Moments 49 | */ 50 | template 51 | class FilterInterface 52 | : public Descriptor 53 | { 54 | public: 55 | /** 56 | * \brief State type provided by the filter specialization 57 | * 58 | * The filter specialization traits must provide the \c State type. The 59 | * The filter specialization traits must provide the \c State type. The 60 | * state type is commonly either provided by the process model or the filter 61 | * self. 62 | */ 63 | typedef typename Traits::State State; 64 | 65 | /** 66 | * \brief Input control type provided by the filter specialization 67 | * 68 | * The filter specialization traits must provide the \c Input control type. 69 | * The \c Input type is commonly either provided by the process model or the 70 | * filter self. 71 | */ 72 | typedef typename Traits::Input Input; 73 | 74 | /** 75 | * \brief Obsrv (Observation) type provided by the filter specialization 76 | * 77 | * The filter specialization traits must provide the \c Obsrv type. 78 | * The \c Obsrv type is commonly provided by the measurement 79 | * model. 80 | */ 81 | typedef typename Traits::Obsrv Obsrv; 82 | 83 | /** 84 | * \brief Belief type uses by the filter specialization. 85 | * 86 | * The filter specialization uses a suitable state distribution. By 87 | * convension, the Belief must implement the Moments 88 | * interface which provides the first two moments. 89 | */ 90 | typedef typename Traits::Belief Belief; 91 | 92 | /** 93 | * Predicts the distribution over the state for the next time step 94 | * 95 | * \param prior_belief Prior state distribution 96 | * \param input Control input argument 97 | * \param predicted_belief Predicted state distribution 98 | */ 99 | virtual void predict(const Belief& prior_belief, 100 | const Input& input, 101 | Belief& predicted_belief) = 0; 102 | 103 | /** 104 | * Updates a predicted state given an observation 105 | * 106 | * \param predicted_belief Predicted state distribution 107 | * \param observation Latest observation 108 | * \param posterior_belief Updated posterior state distribution 109 | */ 110 | virtual void update(const Belief& predicted_belief, 111 | const Obsrv& obsrv, 112 | Belief& posterior_belief) = 0; 113 | }; 114 | 115 | } 116 | -------------------------------------------------------------------------------- /include/fl/filter/gaussian/gaussian_filter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file gaussian_filter.hpp 16 | * \date October 2014 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include "transform/unscented_transform.hpp" 24 | #include "transform/monte_carlo_transform.hpp" 25 | 26 | #include "quadrature/sigma_point_quadrature.hpp" 27 | #include "quadrature/unscented_quadrature.hpp" 28 | 29 | #include "gaussian_filter_linear.hpp" 30 | #include "gaussian_filter_nonlinear.hpp" 31 | #include "gaussian_filter_nonlinear_generic.hpp" 32 | -------------------------------------------------------------------------------- /include/fl/filter/gaussian/prediction_policy/sigma_point_additive_prediction_policy.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file sigma_point_additive_prediction_policy.hpp 16 | * \date July 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace fl 32 | { 33 | 34 | // Forward declarations 35 | template class SigmaPointPredictPolicy; 36 | 37 | template < 38 | typename SigmaPointQuadrature, 39 | typename AdditiveTransitionFunction 40 | > 41 | class SigmaPointPredictPolicy< 42 | SigmaPointQuadrature, 43 | Additive> 44 | : public Descriptor 45 | { 46 | public: 47 | typedef typename AdditiveTransitionFunction::State State; 48 | typedef typename AdditiveTransitionFunction::Input Input; 49 | 50 | enum : signed int 51 | { 52 | NumberOfPoints = SigmaPointQuadrature 53 | ::number_of_points(SizeOf::Value) 54 | }; 55 | 56 | typedef PointSet StatePointSet; 57 | 58 | template < 59 | typename Belief 60 | > 61 | void operator()(const AdditiveTransitionFunction& 62 | additive_transition_function, 63 | const SigmaPointQuadrature& quadrature, 64 | const Belief& prior_belief, 65 | const Input& u, 66 | Belief& predicted_belief) 67 | { 68 | auto f = [&](const State& x) 69 | { 70 | return additive_transition_function.expected_state(x, u); 71 | }; 72 | 73 | quadrature.propergate_gaussian(f, prior_belief, Y, Z); 74 | 75 | /* 76 | * Obtain the centered points matrix of the prediction. The columns of 77 | * this matrix are the predicted points with zero mean. That is, the 78 | * sum of the columns in P is zero. 79 | * 80 | * P = [X_r[1]-mu_r X_r[2]-mu_r ... X_r[n]-mu_r] 81 | * 82 | * with weighted mean 83 | * 84 | * mu_r = Sum w_mean[i] X_r[i] 85 | */ 86 | auto X_c = Z.centered_points(); 87 | 88 | /* 89 | * Obtain the weights of point as a vector 90 | * 91 | * W = [w_cov[1] w_cov[2] ... w_cov[n]] 92 | * 93 | * Note that the covariance weights are used. 94 | */ 95 | auto W = Z.covariance_weights_vector(); 96 | 97 | /* 98 | * Compute and set the moments 99 | * 100 | * The first moment is simply the weighted mean of points. 101 | * The second centered moment is determined by 102 | * 103 | * C = Sum W[i,i] * (X_r[i]-mu_r)(X_r[i]-mu_r)^T 104 | * = P * W * P^T 105 | * 106 | * given that W is the diagonal matrix 107 | */ 108 | predicted_belief.dimension(prior_belief.dimension()); 109 | predicted_belief.mean(Z.mean()); 110 | predicted_belief.covariance( 111 | X_c * W.asDiagonal() * X_c.transpose() 112 | + additive_transition_function.noise_covariance()); 113 | } 114 | 115 | virtual std::string name() const 116 | { 117 | return "SigmaPointPredictPolicy<" 118 | + this->list_arguments( 119 | "SigmaPointQuadrature", 120 | "Additive") 121 | + ">"; 122 | } 123 | 124 | virtual std::string description() const 125 | { 126 | return "Sigma Point based filter prediction policy for state " 127 | "transition model with additive noise"; 128 | } 129 | 130 | protected: 131 | StatePointSet Y; 132 | StatePointSet Z; 133 | }; 134 | 135 | } 136 | 137 | 138 | -------------------------------------------------------------------------------- /include/fl/filter/gaussian/quadrature/unscented_quadrature.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file unscented_quadrature.hpp 16 | * \date July 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | 26 | namespace fl 27 | { 28 | 29 | class UnscentedQuadrature 30 | : public SigmaPointQuadrature 31 | { 32 | public: 33 | /** 34 | * Creates a UnscentedQuadrature 35 | * 36 | * \param alpha UT Scaling parameter alpha (distance to the mean) 37 | * \param beta UT Scaling parameter beta (2.0 is optimal for Gaussian) 38 | * \param kappa UT Scaling parameter kappa (higher order parameter) 39 | */ 40 | UnscentedQuadrature(Real alpha = 1.0, Real beta = 2., Real kappa = 0.) 41 | : SigmaPointQuadrature( 42 | UnscentedTransform(alpha, beta, kappa)) 43 | { } 44 | }; 45 | 46 | } 47 | -------------------------------------------------------------------------------- /include/fl/filter/gaussian/update_policy/sigma_point_additive_uncorrelated_update_policy.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file sigma_point_additive_uncorrelated_update_policy.hpp 16 | * \date July 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace fl 32 | { 33 | 34 | // Forward declarations 35 | template class SigmaPointUpdatePolicy; 36 | 37 | template < 38 | typename SigmaPointQuadrature, 39 | typename AdditiveUncorrelatedObsrvFunction 40 | > 41 | class SigmaPointUpdatePolicy< 42 | SigmaPointQuadrature, 43 | AdditiveUncorrelated> 44 | : public Descriptor 45 | { 46 | public: 47 | typedef typename AdditiveUncorrelatedObsrvFunction::State State; 48 | typedef typename AdditiveUncorrelatedObsrvFunction::Obsrv Obsrv; 49 | 50 | enum : signed int 51 | { 52 | NumberOfPoints = 53 | SigmaPointQuadrature::number_of_points(SizeOf::Value) 54 | }; 55 | 56 | // static_assert(false, "Just implementing this ........"); 57 | 58 | typedef PointSet StatePointSet; 59 | typedef PointSet ObsrvPointSet; 60 | 61 | template < 62 | typename Belief 63 | > 64 | void operator()(const AdditiveUncorrelatedObsrvFunction& obsrv_function, 65 | const SigmaPointQuadrature& quadrature, 66 | const Belief& prior_belief, 67 | const Obsrv& obsrv, 68 | Belief& posterior_belief) 69 | { 70 | auto&& h = [&](const State& x) 71 | { 72 | return obsrv_function.expected_observation(x); 73 | }; 74 | 75 | quadrature.propergate_gaussian(h, prior_belief, X, Z); 76 | 77 | auto R_inv = 78 | obsrv_function 79 | .noise_diagonal_covariance() 80 | .diagonal() 81 | .cwiseInverse() 82 | .eval(); 83 | 84 | auto W_inv = 85 | X.covariance_weights_vector() 86 | .cwiseInverse() 87 | .eval(); 88 | 89 | auto&& prediction = Z.center(); 90 | auto&& Y_c = Z.points(); 91 | auto&& X_c = X.centered_points(); 92 | 93 | auto innovation = (obsrv - prediction).eval(); 94 | 95 | auto C = (Y_c.transpose() * R_inv.asDiagonal() * Y_c).eval(); 96 | C += W_inv.asDiagonal(); 97 | C = C.inverse(); 98 | 99 | auto correction = ( 100 | X_c * C * Y_c.transpose() * R_inv.asDiagonal() * innovation).eval(); 101 | 102 | posterior_belief.dimension(prior_belief.dimension()); 103 | posterior_belief.mean(X.mean() + correction); 104 | posterior_belief.covariance(X_c * C * X_c.transpose()); 105 | } 106 | 107 | virtual std::string name() const 108 | { 109 | return "SigmaPointUpdatePolicy<" 110 | + this->list_arguments( 111 | "SigmaPointQuadrature", 112 | "AdditiveUncorrelated") 113 | + ">"; 114 | } 115 | 116 | virtual std::string description() const 117 | { 118 | return "Sigma Point based filter update policy for observation model" 119 | " with additive uncorrelated noise"; 120 | } 121 | 122 | protected: 123 | StatePointSet X; 124 | ObsrvPointSet Z; 125 | }; 126 | 127 | } 128 | 129 | 130 | 131 | -------------------------------------------------------------------------------- /include/fl/filter/gaussian/update_policy/sigma_point_additive_update_policy.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file sigma_point_additive_update_policy.hpp 16 | * \date July 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace fl 32 | { 33 | 34 | // Forward declarations 35 | template class SigmaPointUpdatePolicy; 36 | 37 | template < 38 | typename SigmaPointQuadrature, 39 | typename AdditiveSensorFunction 40 | > 41 | class SigmaPointUpdatePolicy< 42 | SigmaPointQuadrature, 43 | Additive> 44 | : public Descriptor 45 | { 46 | public: 47 | typedef typename AdditiveSensorFunction::State State; 48 | typedef typename AdditiveSensorFunction::Obsrv Obsrv; 49 | 50 | enum : signed int 51 | { 52 | NumberOfPoints = 53 | SigmaPointQuadrature::number_of_points(SizeOf::Value) 54 | }; 55 | 56 | typedef PointSet StatePointSet; 57 | typedef PointSet ObsrvPointSet; 58 | 59 | template < 60 | typename Belief 61 | > 62 | void operator()(const AdditiveSensorFunction& obsrv_function, 63 | const SigmaPointQuadrature& quadrature, 64 | const Belief& prior_belief, 65 | const Obsrv& obsrv, 66 | Belief& posterior_belief) 67 | { 68 | auto&& h = [&](const State& x) 69 | { 70 | return obsrv_function.expected_observation(x); 71 | }; 72 | 73 | quadrature.propergate_gaussian(h, prior_belief, X, Z); 74 | 75 | auto&& prediction = Z.center(); 76 | auto&& Z_c = Z.points(); 77 | auto&& W = X.covariance_weights_vector(); 78 | auto&& X_c = X.centered_points(); 79 | 80 | auto innovation = (obsrv - prediction).eval(); 81 | auto cov_xx = (X_c * W.asDiagonal() * X_c.transpose()).eval(); 82 | auto cov_yy = (Z_c * W.asDiagonal() * Z_c.transpose() 83 | + obsrv_function.noise_covariance()).eval(); 84 | auto cov_xy = (X_c * W.asDiagonal() * Z_c.transpose()).eval(); 85 | auto K = (cov_xy * cov_yy.inverse()).eval(); 86 | 87 | posterior_belief.dimension(prior_belief.dimension()); 88 | posterior_belief.mean(X.mean() + K * innovation); 89 | posterior_belief.covariance(cov_xx - K * cov_yy * K.transpose()); 90 | } 91 | 92 | virtual std::string name() const 93 | { 94 | return "SigmaPointUpdatePolicy<" 95 | + this->list_arguments( 96 | "SigmaPointQuadrature", 97 | "Additive") 98 | + ">"; 99 | } 100 | 101 | virtual std::string description() const 102 | { 103 | return "Sigma Point based filter update policy for observation model" 104 | " with additive noise"; 105 | } 106 | 107 | protected: 108 | StatePointSet X; 109 | ObsrvPointSet Z; 110 | }; 111 | 112 | } 113 | 114 | 115 | 116 | -------------------------------------------------------------------------------- /include/fl/model/adaptive_model.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file adaptive_model.hpp 16 | * \date Febuary 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | 25 | namespace fl 26 | { 27 | 28 | /** 29 | * \ingroup transitions 30 | * \ingroup sensors 31 | * \interface AdaptiveModel 32 | * 33 | * \brief The AdaptiveModel interface represents the base of any model which 34 | * provides a variable set of parameters \f$\theta\f$ such that the model \f$y = 35 | * h(x, w)\f$ becomes \f$h(x, w, \theta)\f$. Here \f$x\f$ is the state, 36 | * \f$w\sim {\cal N}(0, 1)\f$ is a white noise term. 37 | * 38 | * Strictly speeking, the parameters \f$\theta\f$ are simply a part of the state 39 | * \f$x\f$. However, this separation is particulalry useful if the adaptation of 40 | * the paramters \f$\theta\f$ is optional. That is, \f$\theta\f$ may as well 41 | * remain unchanged during run-time without modifying the variable \f$x\f$ nor 42 | * forcing the extension of \f$x\f$ by the \f$\theta\f$. 43 | * 44 | * \note 45 | * 46 | * \tparam Parameter Model variable parameters \f$\theta\f$. It represents a 47 | * subset of the models parameter which are subject to 48 | * changes during exectution time. That is, \f$\theta\f$ 49 | * does not represent all model parameters but only those 50 | * which may be adapted on-line such as in adaptive 51 | * estiamtion in which the parameters are estimated along 52 | * the state \f$x\f$ \todo REF. 53 | */ 54 | template 55 | class AdaptiveModel 56 | : public internal::AdaptiveModelType 57 | { 58 | public: 59 | typedef internal::AdaptiveModelType AdaptivityType; 60 | 61 | public: 62 | /** 63 | * \return Model parameters \f$\theta\f$. 64 | */ 65 | virtual const Param& param() const = 0; 66 | 67 | /** 68 | * Sets the adaptive parameters of the model. 69 | * 70 | * \param param New model parameter values \f$\theta\f$ 71 | */ 72 | virtual void param(Param params) = 0; 73 | 74 | /** 75 | * \return Parameter variable dimension: \f$dim(\theta)\f$ 76 | */ 77 | virtual int param_dimension() const = 0; 78 | }; 79 | 80 | } 81 | 82 | 83 | -------------------------------------------------------------------------------- /include/fl/model/additive_noise_model.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file additive_noise_model.hpp 16 | * \date July 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | namespace fl 29 | { 30 | 31 | template 32 | class AdditiveNoiseModel 33 | : private internal::AdditiveNoiseModelType 34 | { 35 | public: 36 | typedef internal::AdditiveNoiseModelType Type; 37 | 38 | /** 39 | * Noise model matrix \f$N_t\f$ 40 | */ 41 | typedef typename NoiseDensity::SecondMoment NoiseMatrix; 42 | public: 43 | /** 44 | * \brief Overridable default destructor 45 | */ 46 | virtual ~AdditiveNoiseModel() noexcept { } 47 | 48 | virtual NoiseMatrix noise_matrix() const = 0; 49 | virtual NoiseMatrix noise_covariance() const = 0; 50 | }; 51 | 52 | } 53 | 54 | 55 | -------------------------------------------------------------------------------- /include/fl/model/additive_uncorrelated_noise_model.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file additive_uncorrelated_noise_model.hpp 16 | * \date July 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | namespace fl 29 | { 30 | 31 | template 32 | class AdditiveUncorrelatedNoiseModel 33 | : private internal::AdditiveUncorrelatedNoiseModelType 34 | { 35 | public: 36 | typedef typename NoiseDensity::SecondMoment NoiseMatrix; 37 | public: 38 | virtual NoiseMatrix noise_diagonal_matrix() const = 0; 39 | virtual NoiseMatrix noise_diagonal_covariance() const = 0; 40 | }; 41 | 42 | } 43 | 44 | 45 | -------------------------------------------------------------------------------- /include/fl/model/sensor/interface/additive_sensor_function.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file additive_sensor_function.hpp 16 | * \date June 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | namespace fl 29 | { 30 | 31 | template < 32 | typename Obsrv, 33 | typename State, 34 | typename NoiseDensity, 35 | int Id = 0 36 | > 37 | class AdditiveSensorFunction 38 | : public SensorFunction, 39 | public AdditiveNoiseModel 40 | { 41 | public: 42 | typedef typename NoiseDensity::Variate Noise; 43 | typedef internal::AdditiveNoiseModelType Type; 44 | typedef SensorFunction FunctionInterface; 45 | typedef AdditiveNoiseModel AdditiveInterface; 46 | 47 | using AdditiveInterface::noise_matrix; 48 | 49 | public: 50 | /** 51 | * \brief Overridable default destructor 52 | */ 53 | virtual ~AdditiveSensorFunction() noexcept { } 54 | 55 | /** 56 | * Evaluates the model function \f$y = h(x, w)\f$ where \f$x\f$ is the state 57 | * and \f$w\sim {\cal N}(0, 1)\f$ is a white noise parameter. Put 58 | * differently, \f$y = h(x, w)\f$ is a sample from the conditional model 59 | * distribution \f$p(y \mid x)\f$. 60 | * 61 | * \param state The state variable \f$x\f$ 62 | * \param noise The noise term \f$w\f$ 63 | * \param delta_time Prediction time 64 | */ 65 | virtual Obsrv expected_observation(const State& state) const = 0; 66 | 67 | Obsrv observation(const State& state, const Noise& noise) const override 68 | { 69 | return expected_observation(state) + noise_matrix() * noise; 70 | } 71 | }; 72 | 73 | } 74 | 75 | 76 | -------------------------------------------------------------------------------- /include/fl/model/sensor/interface/additive_uncorrelated_sensor_function.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file additive_uncorrelated_sensor_function.hpp 16 | * \date June 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | namespace fl 29 | { 30 | 31 | template < 32 | typename Obsrv, 33 | typename State, 34 | typename Noise, 35 | int Id = 0 36 | > 37 | class AdditiveUncorrelatedSensorFunction 38 | : public AdditiveSensorFunction, 39 | public AdditiveUncorrelatedNoiseModel, 40 | private internal::AdditiveUncorrelatedNoiseModelType 41 | { 42 | public: 43 | /** 44 | * \brief Overridable default destructor 45 | */ 46 | virtual ~AdditiveUncorrelatedSensorFunction() noexcept { } 47 | }; 48 | 49 | } 50 | 51 | 52 | -------------------------------------------------------------------------------- /include/fl/model/sensor/interface/sensor_function.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file sensor_function.hpp 16 | * \date October 2014 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | 26 | namespace fl 27 | { 28 | 29 | /** 30 | * \ingroup sensors 31 | */ 32 | template < 33 | typename Obsrv_, 34 | typename State_, 35 | typename Noise_, 36 | int Id = 0 37 | > 38 | class SensorFunction 39 | : private internal::NonAdditiveNoiseModelType 40 | { 41 | public: 42 | typedef internal::NonAdditiveNoiseModelType Type; 43 | 44 | typedef Obsrv_ Obsrv; 45 | typedef State_ State; 46 | typedef Noise_ Noise; 47 | 48 | /** 49 | * \brief Overridable default destructor 50 | */ 51 | virtual ~SensorFunction() noexcept { } 52 | 53 | /** 54 | * Evaluates the model function \f$y = h(x, w)\f$ where \f$x\f$ is the state 55 | * and \f$w\sim {\cal N}(0, 1)\f$ is a white noise parameter. Put 56 | * differently, \f$y = h(x, w)\f$ is a sample from the conditional model 57 | * distribution \f$p(y \mid x)\f$. 58 | * 59 | * \param state The state variable \f$x\f$ 60 | * \param noise The noise term \f$w\f$ 61 | * \param delta_time Prediction time 62 | */ 63 | virtual Obsrv observation(const State& state, 64 | const Noise& noise) const = 0; 65 | 66 | /** 67 | * \brief Returns the dimension of the state variable \f$x\f$ 68 | */ 69 | virtual int state_dimension() const = 0; 70 | 71 | /** 72 | * \brief Returns the dimension of the noise term \f$w\f$ 73 | */ 74 | virtual int noise_dimension() const = 0; 75 | 76 | /** 77 | * \brief Returns the dimension of the measurement \f$h(x, w)\f$ 78 | */ 79 | virtual int obsrv_dimension() const = 0; 80 | 81 | /** 82 | * \return Model id number 83 | * 84 | * In case of multiple sensors of the same kind, this function returns the 85 | * id of the individual model. 86 | */ 87 | virtual int id() const { return Id; } 88 | 89 | /** 90 | * \brief Sets the model id for mulit-sensor use 91 | * 92 | * In some cases a single observation model may be used for multiple sensor 93 | * of the same kind. It often suffices to alter the sensor id before 94 | * evaluating the model. 95 | * 96 | * \param new_id Model's new ID 97 | */ 98 | virtual void id(int) { /* const ID */ } 99 | }; 100 | 101 | } 102 | 103 | 104 | -------------------------------------------------------------------------------- /include/fl/model/sensor/joint_sensor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | * 13 | */ 14 | 15 | /** 16 | * \file joint_sensor.hpp 17 | * \date Febuary 2015 18 | * \author Jan Issac (jan.issac@gmail.com) 19 | */ 20 | 21 | #pragma once 22 | 23 | 24 | #include 25 | #include 26 | 27 | 28 | -------------------------------------------------------------------------------- /include/fl/model/sensor/linear_cauchy_sensor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file linear_cauchy_sensor.hpp 16 | * \date August 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | 31 | namespace fl 32 | { 33 | 34 | /** 35 | * \ingroup sensors 36 | */ 37 | template 38 | class LinearCauchySensor 39 | : public SensorFunction< 40 | Obsrv, 41 | State, 42 | typename CauchyDistribution::StandardVariate>, // noise 43 | public SensorDensity, 44 | public Descriptor 45 | { 46 | public: 47 | typedef typename CauchyDistribution::StandardVariate Noise; 48 | typedef typename CauchyDistribution::SecondMoment NoiseMatrix; 49 | 50 | /** 51 | * Observation model sensor matrix \f$H_t\f$ use in 52 | * 53 | * \f$ y_t = H_t x_t + N_t v_t \f$ 54 | */ 55 | typedef Eigen::Matrix< 56 | typename State::Scalar, 57 | SizeOf::Value, 58 | SizeOf::Value 59 | > SensorMatrix; 60 | 61 | /** 62 | * Constructs a linear gaussian observation model 63 | * 64 | * \param obsrv_dim observation dimension if dynamic size 65 | * \param state_dim state dimension if dynamic size 66 | */ 67 | explicit 68 | LinearCauchySensor( 69 | int obsrv_dim = DimensionOf(), 70 | int state_dim = DimensionOf()) 71 | : sensor_matrix_(SensorMatrix::Identity(obsrv_dim, state_dim)), 72 | density_(obsrv_dim) 73 | { 74 | assert(obsrv_dim > 0); 75 | assert(state_dim > 0); 76 | } 77 | 78 | /** 79 | * \brief Overridable default destructor 80 | */ 81 | virtual ~LinearCauchySensor() noexcept { } 82 | 83 | Obsrv observation(const State& state, const Noise& noise) const override 84 | { 85 | return sensor_matrix_ * state + density_.map_standard_normal(noise); 86 | } 87 | 88 | Real log_probability(const Obsrv& obsrv, const State& state) const override 89 | { 90 | density_.location(sensor_matrix_ * state); 91 | 92 | return density_.log_probability(obsrv); 93 | } 94 | 95 | const SensorMatrix& sensor_matrix() const override 96 | { 97 | return sensor_matrix_; 98 | } 99 | 100 | NoiseMatrix noise_covariance() const override 101 | { 102 | return density_.covariance(); 103 | } 104 | 105 | int obsrv_dimension() const override 106 | { 107 | return sensor_matrix_.rows(); 108 | } 109 | 110 | int noise_dimension() const override 111 | { 112 | return density_.standard_variate_dimension(); 113 | } 114 | 115 | int state_dimension() const override 116 | { 117 | return sensor_matrix_.cols(); 118 | } 119 | 120 | virtual void sensor_matrix(const SensorMatrix& sensor_mat) 121 | { 122 | sensor_matrix_ = sensor_mat; 123 | } 124 | 125 | virtual void noise_covariance(const NoiseMatrix& noise_mat) 126 | { 127 | density_.scaling_matrix(noise_mat); 128 | } 129 | 130 | virtual SensorMatrix create_sensor_matrix() const 131 | { 132 | auto H = sensor_matrix(); 133 | H.setIdentity(); 134 | return H; 135 | } 136 | 137 | virtual std::string name() const 138 | { 139 | return "LinearCauchySensor"; 140 | } 141 | 142 | virtual std::string description() const 143 | { 144 | return "Linear cauchy observation model with Cauchy distribution noise"; 145 | } 146 | 147 | private: 148 | SensorMatrix sensor_matrix_; 149 | mutable CauchyDistribution density_; 150 | }; 151 | 152 | } 153 | 154 | 155 | -------------------------------------------------------------------------------- /include/fl/model/sensor/linear_gaussian_sensor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file linear_gaussian_sensor.hpp 16 | * \date October 2014 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | namespace fl 29 | { 30 | 31 | /** 32 | * \ingroup sensors 33 | * 34 | * This represents the linear gaussian observation model \f$p(y_t \mid x_t)\f$ 35 | * governed by the linear equation 36 | * 37 | * \f$ y_t = H_t x_t + N_t v_t \f$ 38 | * 39 | * where \f$ y_t\f$ is the observation, \f$ x_t\f$ is the state. The matrix 40 | * \f$H_t\f$ is the sensor model mapping the state into the observation space. 41 | * The vector \f$ v_t \sim {\cal N}(v_t; 0, I)\f$ is a standard normal variate 42 | * which is mapped into the the observation space via the noise model matrix 43 | * \f$N_t\f$. Any Gaussian noise \f$\tilde{v}_t \sim {\cal N}(\tilde{v}_t ; 0, 44 | * R_t) \f$ can be represented via \f$\tilde{v}_t = N_t v_t\f$ with\f$N_t 45 | * = \sqrt{R_t}\f$. Hence, the linear equation may be restated as the more 46 | * familiar but equivalent form 47 | * 48 | * \f$ y_t = H_t x_t + \tilde{v}_t \f$. 49 | */ 50 | template 51 | class LinearGaussianSensor 52 | : public LinearSensor>, 53 | public Descriptor 54 | { 55 | public: 56 | /** 57 | * Constructs a linear gaussian observation model 58 | * \param obsrv_dim observation dimension if dynamic size 59 | * \param state_dim state dimension if dynamic size 60 | */ 61 | explicit 62 | LinearGaussianSensor(int obsrv_dim = DimensionOf(), 63 | int state_dim = DimensionOf()) 64 | : LinearSensor>( 65 | obsrv_dim, state_dim) 66 | { } 67 | 68 | virtual std::string name() const 69 | { 70 | return "LinearGaussianSensor"; 71 | } 72 | 73 | virtual std::string description() const 74 | { 75 | return "Linear observation model with additive Gaussian noise"; 76 | } 77 | }; 78 | 79 | } 80 | 81 | 82 | -------------------------------------------------------------------------------- /include/fl/model/sensor/uniform_sensor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file uniform_sensor.hpp 16 | * \date September 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace fl 30 | { 31 | 32 | template 33 | class UniformSensor 34 | : public SensorFunction, 35 | public SensorDensity, 36 | public Descriptor 37 | { 38 | public: 39 | typedef Vector1d Obsrv; 40 | typedef Vector1d Noise; 41 | typedef State_ State; 42 | 43 | public: 44 | UniformSensor( 45 | Real min_value, 46 | Real max_value, 47 | int state_dim = DimensionOf::Value) 48 | : state_dim_(state_dim), 49 | density_(min_value, max_value) 50 | { } 51 | 52 | Real log_probability(const Obsrv& obsrv, const State& state) const override 53 | { 54 | return density_.log_probability(obsrv); 55 | } 56 | 57 | Real probability(const Obsrv& obsrv, const State& state) const override 58 | { 59 | return density_.probability(obsrv); 60 | } 61 | 62 | Obsrv observation(const State& state, const Noise& noise) const override 63 | { 64 | Obsrv y = density_.map_standard_normal(noise); 65 | return y; 66 | } 67 | 68 | virtual int obsrv_dimension() const { return 1; } 69 | virtual int noise_dimension() const { return 1; } 70 | virtual int state_dimension() const { return state_dim_; } 71 | 72 | virtual std::string name() const 73 | { 74 | return "UniformSensor"; 75 | } 76 | 77 | virtual std::string description() const 78 | { 79 | return "UniformSensor"; 80 | } 81 | 82 | private: 83 | int state_dim_; 84 | UniformDistribution density_; 85 | }; 86 | 87 | } 88 | -------------------------------------------------------------------------------- /include/fl/model/transition/binary_transition_density.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file binary_transition_density.hpp 16 | * \date June 2015 17 | * \author Manuel Wuthrich (manuel.wuthrich@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | 26 | 27 | 28 | namespace fl 29 | { 30 | 31 | class BinaryTransitionDensity /// \todo not really a density since it is discrete 32 | { 33 | public: 34 | BinaryTransitionDensity(const Real& p_1to1, 35 | const Real& p_0to1): p_1to1_(p_1to1), 36 | p_0to1_(p_0to1) 37 | { 38 | /// \todo: this case could be handled properly, but it would be 39 | /// a little bit more involved 40 | if(p_0to1 > p_1to1) 41 | { 42 | std::cout << "the case of p_0to1 > p_1to1 is not handled in the " 43 | << "binary transition density." << std::endl; 44 | } 45 | } 46 | 47 | Real probability(const bool& next_state, 48 | const bool& state, 49 | const Real& dt) 50 | { 51 | Real a = std::pow(p_1to1_ - p_0to1_, dt); 52 | Real prob_1 = a * Real(state) 53 | + (a - 1.) * p_0to1_ / (p_1to1_ - 1. - p_0to1_); 54 | 55 | // limit of p_0to1_ -> 0 and p_1to1_ -> 1 56 | if(!std::isfinite(prob_1)) prob_1 = Real(state); 57 | 58 | return next_state ? prob_1 : 1. - prob_1; 59 | } 60 | 61 | 62 | private: 63 | Real p_1to1_; 64 | Real p_0to1_; 65 | }; 66 | 67 | 68 | } 69 | 70 | 71 | -------------------------------------------------------------------------------- /include/fl/model/transition/interface/additive_transition_function.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file additive_transition_function.hpp 16 | * \date June 2015 17 | * \author Manuel Wuthrich (manuel.wuthrich@gmail.com) 18 | * \author Jan Issac (jan.issac@gmail.com) 19 | */ 20 | 21 | #pragma once 22 | 23 | 24 | #include 25 | 26 | #include 27 | 28 | namespace fl 29 | { 30 | 31 | template < 32 | typename State, 33 | typename Noise, 34 | typename Input, 35 | int Id = 0 36 | > 37 | class AdditiveTransitionFunction 38 | : public TransitionFunction, 39 | public internal::AdditiveNoiseModelType 40 | { 41 | public: 42 | typedef internal::AdditiveNoiseModelType Type; 43 | 44 | typedef TransitionFunction FunctionInterface; 45 | 46 | /** 47 | * Noise model matrix \f$N_t\f$ 48 | */ 49 | typedef Eigen::Matrix< 50 | typename Noise::Scalar, 51 | SizeOf::Value, 52 | SizeOf::Value 53 | > NoiseMatrix; 54 | 55 | /** 56 | * Noise covariance matrix \f$N_t\f$ 57 | */ 58 | typedef Eigen::Matrix< 59 | typename Noise::Scalar, 60 | SizeOf::Value, 61 | SizeOf::Value 62 | > NoiseCovariance; 63 | 64 | public: 65 | /** 66 | * \brief Overridable default destructor 67 | */ 68 | virtual ~AdditiveTransitionFunction() noexcept { } 69 | 70 | virtual State expected_state(const State& prev_state, 71 | const Input& input) const = 0; 72 | 73 | virtual const NoiseMatrix& noise_matrix() const = 0; 74 | virtual const NoiseCovariance& noise_covariance() const = 0; 75 | 76 | virtual State state(const State& prev_state, 77 | const Noise& noise, 78 | const Input& input) const 79 | { 80 | return expected_state(prev_state, input) + noise_matrix() * noise; 81 | } 82 | }; 83 | 84 | } 85 | 86 | 87 | -------------------------------------------------------------------------------- /include/fl/model/transition/interface/additive_uncorrelated_transition_function.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file additive_uncorrelated_transition_function.hpp 16 | * \date June 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | 25 | #include 26 | 27 | namespace fl 28 | { 29 | 30 | template < 31 | typename State, 32 | typename Input, 33 | int Id = 0 34 | > 35 | class AdditiveUncorrelatedTransitionFunction 36 | : public AdditiveTransitionFunction 37 | { 38 | public: 39 | typedef AdditiveTransitionFunction< 40 | State, State, Input, Id 41 | > AdditiveFunctionInterface; 42 | 43 | typedef Eigen::DiagonalMatrix< 44 | typename State::Scalar, 45 | SizeOf::Value 46 | > NoiseDiagonal; 47 | 48 | public: 49 | /** 50 | * \brief Overridable default destructor 51 | */ 52 | virtual ~AdditiveUncorrelatedTransitionFunction() noexcept { } 53 | 54 | virtual const NoiseDiagonal& noise_diagonal_matrix() const = 0; 55 | virtual const NoiseDiagonal& noise_diagonal_covariance() const = 0; 56 | 57 | 58 | /* TODO: override state() function using noise_diagonal_matrix() */ 59 | }; 60 | 61 | } 62 | -------------------------------------------------------------------------------- /include/fl/model/transition/interface/transition_density.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file transition_density.hpp 16 | * \date June 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | * \author Manuel Wuthrich (manuel.wuthrich@gmail.com) 19 | */ 20 | 21 | #pragma once 22 | 23 | 24 | #include 25 | #include 26 | 27 | namespace fl 28 | { 29 | 30 | template < 31 | typename State_, 32 | typename Input_, 33 | int BatchSize = Eigen::Dynamic 34 | > 35 | class TransitionDensity 36 | { 37 | public: 38 | typedef State_ State; 39 | typedef Input_ Input; 40 | 41 | typedef Eigen::Array StateArray; 42 | typedef Eigen::Array InputArray; 43 | typedef Eigen::Array ValueArray; 44 | 45 | public: 46 | /** 47 | * \brief Overridable default destructor 48 | */ 49 | virtual ~TransitionDensity() noexcept { } 50 | 51 | 52 | /// \todo should add the unnormalized log probability interface 53 | 54 | 55 | virtual Real log_probability(const State& state, 56 | const State& cond_state, 57 | const Input& cond_input) const = 0; 58 | 59 | /** 60 | * \return Dimension of the state variable $\fx_t\f$ 61 | */ 62 | virtual int state_dimension() const = 0; 63 | 64 | /** 65 | * \return Dimension of the input \f$u_t\f$ 66 | */ 67 | virtual int input_dimension() const = 0; 68 | 69 | 70 | virtual Real probability(const State& state, 71 | const State& cond_state, 72 | const Input& cond_input) const 73 | { 74 | return std::exp(log_probability(state, 75 | cond_state, 76 | cond_input)); 77 | } 78 | 79 | virtual ValueArray log_probabilities(const StateArray& states, 80 | const StateArray& cond_states, 81 | const InputArray& cond_inputs) const 82 | { 83 | assert(states.size() == cond_inputs.size()); 84 | 85 | auto probs = ValueArray(states.size()); 86 | 87 | for (int i = 0; i < states.size(); ++i) 88 | { 89 | probs[i] = log_probability(states[i], 90 | cond_states[i], 91 | cond_inputs[i]); 92 | } 93 | 94 | return probs; 95 | } 96 | 97 | virtual ValueArray probabilities(const StateArray& states, 98 | const StateArray& cond_states, 99 | const InputArray& cond_inputs) 100 | { 101 | return log_probabilities(states, cond_states, cond_inputs).exp(); 102 | } 103 | }; 104 | 105 | } 106 | 107 | 108 | -------------------------------------------------------------------------------- /include/fl/model/transition/joint_transition.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file joint_transition.hpp 16 | * \date Febuary 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | 26 | 27 | -------------------------------------------------------------------------------- /include/fl/util/assertions.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file assertions.hpp 16 | * \date October 2014 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | 26 | /** 27 | * \ingroup macros 28 | * \internal 29 | * 30 | * This variadic macro performs a compile time derivation assertion. The first 31 | * argument is the derived type which is being tested whether it implements a 32 | * base type. The base type is given as the second argument list. 33 | * 34 | * __VA_ARGS__ was used as a second parameter to enable passing template 35 | * specialization to the macro. 36 | * 37 | * Note: The macro requires derived_type to be a single worded type. In 38 | * case of a template specialization, use a typedef. 39 | */ 40 | #define static_assert_base(derived_type, ...)\ 41 | static_assert(( \ 42 | std::is_base_of<__VA_ARGS__, derived_type>::value), \ 43 | #derived_type " must derive from " #__VA_ARGS__); 44 | 45 | #define static_assert_dynamic_sized(matrix)\ 46 | static_assert(matrix::SizeAtCompileTime == Eigen::Dynamic,\ 47 | "Calling a fixed-size function on a dynamic-size one."); 48 | 49 | #define static_assert_const_sized(matrix)\ 50 | static_assert(matrix::SizeAtCompileTime != Eigen::Dynamic,\ 51 | "Calling a dynamic size function on a fixed-size one."); 52 | 53 | 54 | -------------------------------------------------------------------------------- /include/fl/util/math.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file math.hpp 16 | * \date January 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include "math/general_functions.hpp" 24 | #include "math/special_functions.hpp" 25 | #include "math/linear_algebra.hpp" 26 | 27 | 28 | -------------------------------------------------------------------------------- /include/fl/util/math/general_functions.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file general_functions.hpp 16 | * \date January 2015 17 | * \author Manuel Wuthrich (manuel.wuthrich@gmail.com) 18 | * \author Jan Issac (jan.issac@gmail.com) 19 | */ 20 | 21 | #pragma once 22 | 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | namespace fl 29 | { 30 | 31 | /** 32 | * \brief Sigmoid function 33 | * \ingroup general_functions 34 | */ 35 | inline Real sigmoid(const Real& x) 36 | { 37 | return 1.0 / (1.0 + std::exp(-x)); 38 | } 39 | 40 | /** 41 | * \brief log odd 42 | * \ingroup general_functions 43 | */ 44 | inline Real logit(const Real& x) 45 | { 46 | return std::log(x / (1.0 - x)); 47 | } 48 | 49 | 50 | 51 | inline long timesteps(Real discretization_time_step, 52 | Real delta_time) 53 | { 54 | return std::round(delta_time/discretization_time_step); 55 | 56 | // // constexpr return function version 57 | // return (delta_time/discretization_time_step) - 58 | // - int(delta_time/discretization_time_step) > 0.5 ? 59 | // int(delta_time/discretization_time_step) + 1 : 60 | // int(delta_time/discretization_time_step); 61 | } 62 | 63 | 64 | /** 65 | * \ingroup general_functions 66 | * \return True if d is within the specified epsilon bounds 67 | */ 68 | inline bool check_epsilon_bounds(Real d, Real epsilon) 69 | { 70 | return std::fabs(d) < epsilon; 71 | } 72 | 73 | /** 74 | * \ingroup general_functions 75 | * 76 | * \return converts a standard normal variate into a uniformly distributed 77 | * variate u 78 | */ 79 | inline Real normal_to_uniform(Real snv) 80 | { 81 | static const Real sqrt_of_2 = std::sqrt(Real(2)); 82 | 83 | Real u = (1 + std::erf(snv / sqrt_of_2)) / Real(2); 84 | return u; 85 | } 86 | 87 | /** 88 | * \ingroup general_functions 89 | * 90 | * \return converts a standard normal variate into a uniformly distributed 91 | * variate u 92 | */ 93 | inline Real uniform_to_normal(Real u) 94 | { 95 | static const Real sqrt_of_2 = std::sqrt(Real(2)); 96 | 97 | Real snv = fl::erfinv(Real(2) * u - 1) * sqrt_of_2; 98 | return snv; 99 | } 100 | 101 | } 102 | 103 | 104 | -------------------------------------------------------------------------------- /include/fl/util/meta.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file meta.hpp 16 | * \date October 2014 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include "meta/size_deduction.hpp" 24 | #include "meta/index_sequence.hpp" 25 | #include "meta/type_sequence.hpp" 26 | #include "meta/options_argument.hpp" 27 | #include "meta/operator/join.hpp" 28 | #include "meta/operator/multiple_of.hpp" 29 | #include "meta/operator/adaptive.hpp" 30 | #include "meta/operator/not_adaptive.hpp" 31 | #include "meta/operator/forward_adaptive.hpp" 32 | #include "meta/operator/use_as.hpp" 33 | 34 | 35 | -------------------------------------------------------------------------------- /include/fl/util/meta/index_sequence.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file index_sequence.hpp 16 | * \date Febuary 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | namespace fl 24 | { 25 | 26 | /** 27 | * \ingroup meta 28 | * 29 | * Represents a sequence of indices IndexSequence<0, 1, 2, 3, ...> 30 | * 31 | * This is particularly useful to expand tuples 32 | * 33 | * \tparam Indices List of indices starting from 0 34 | */ 35 | template struct IndexSequence 36 | { 37 | enum { Size = sizeof...(Indices) }; 38 | }; 39 | 40 | /** 41 | * \ingroup meta 42 | * 43 | * Creates an IndexSequence<0, 1, 2, ...> for a specified size. 44 | */ 45 | template 46 | struct CreateIndexSequence 47 | : CreateIndexSequence 48 | { }; 49 | 50 | /** 51 | * \internal 52 | * \ingroup meta 53 | * 54 | * Terminal specialization CreateIndexSequence 55 | */ 56 | template 57 | struct CreateIndexSequence<0, Indices...> 58 | : IndexSequence 59 | { }; 60 | 61 | } 62 | 63 | 64 | -------------------------------------------------------------------------------- /include/fl/util/meta/operator/forward_adaptive.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file forward_adaptive.hpp 16 | * \date Febuary 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | namespace fl 30 | { 31 | 32 | // Forward declaration 33 | template struct ForwardAdaptive; 34 | 35 | /** 36 | * \ingroup meta 37 | * 38 | * \brief ForwardAdaptive operator injects the AdaptiveModel interface into the 39 | * model if the specified \c Model does not implement it. If the model already 40 | * implements the interface, no changes will be made. 41 | * 42 | * The resulting model of ForwardAdaptive::Type can be used 43 | * within the adaptive context. For instance, the JointSensor<...> 44 | * is an adaptive model and therefore its sum-models must be adaptive too. If 45 | * one of the sub-models does not implement AdaptiveModel it will be 46 | * translated into an adaptive model using the ForwardAdaptive operator. This is 47 | * achieved by applying the NotAdaptive decorating operator on the specific 48 | * model. 49 | */ 50 | template 51 | struct ForwardAdaptive 52 | : ForwardAdaptive< 53 | Model, 54 | Options< 55 | !std::is_base_of::value 56 | & 57 | (std::is_base_of::value | 58 | std::is_base_of::value) 59 | > 60 | > 61 | { }; 62 | 63 | /** 64 | * \internal 65 | * \ingroup meta 66 | * 67 | * ForwardAdaptive specialization forwarding an adaptive model of \c Model which 68 | * is decorated by the NotAdaptive operator 69 | */ 70 | template 71 | struct ForwardAdaptive> 72 | { 73 | typedef NotAdaptive Type; 74 | }; 75 | 76 | /** 77 | * \internal 78 | * \ingroup meta 79 | * 80 | * ForwardAdaptive specialization forwarding the unmodified \c Model which 81 | * already implements the AdaptiveModel interface. 82 | */ 83 | template 84 | struct ForwardAdaptive> 85 | { 86 | typedef Model Type; 87 | }; 88 | 89 | 90 | 91 | } 92 | 93 | 94 | -------------------------------------------------------------------------------- /include/fl/util/meta/operator/multiple_of.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file multiple_of.hpp 16 | * \date Febuary 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | namespace fl 29 | { 30 | 31 | /** 32 | * \ingroup meta 33 | * 34 | * Same as CreateTypeSequence, however with a reversed parameter order. This is 35 | * an attempt to make the use of \c CreateTypeSequence more natural. It also 36 | * allows dynamic sizes if needed. 37 | */ 38 | template 39 | struct MultipleOf 40 | //: CreateTypeSequence::Type> 41 | { 42 | //typedef typename ForwardAdaptive::Type Type; 43 | 44 | enum : signed int { Count = Count_ }; 45 | typedef T Type; 46 | 47 | MultipleOf(const Type& instance, int instance_count = Count) 48 | : instance(instance), 49 | count(instance_count) 50 | { } 51 | 52 | Type instance; 53 | int count; 54 | }; 55 | 56 | } 57 | 58 | 59 | -------------------------------------------------------------------------------- /include/fl/util/meta/operator/use_as.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file use.hpp 16 | * \date July 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace fl 28 | { 29 | 30 | template struct UseAs; 31 | 32 | template 33 | struct UseAs> 34 | { 35 | typedef Model_ Model; 36 | typedef Additive Type; 37 | 38 | static_assert(IsAdditive::Value, "Model noise is not addtive"); 39 | }; 40 | 41 | template 42 | struct UseAs> 43 | { 44 | typedef Model_ Model; 45 | typedef AdditiveUncorrelated Type; 46 | 47 | static_assert( 48 | IsAdditiveUncorrelated::Value, 49 | "Model noise is not of type addtive and uncorrelated"); 50 | }; 51 | 52 | template 53 | struct UseAs> 54 | { 55 | typedef Model_ Model; 56 | typedef NonAdditive Type; 57 | 58 | static_assert( 59 | IsNonAdditive::Value, 60 | "Model noise is not of type non-addtive"); 61 | }; 62 | 63 | } 64 | 65 | 66 | -------------------------------------------------------------------------------- /include/fl/util/meta/options_argument.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file options_argument.hpp 16 | * \date Febuary 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | namespace fl 24 | { 25 | 26 | /** 27 | * \ingroup traits 28 | * 29 | * \brief FilterOptions define a set of options which are used to specialize 30 | * filter algorithms. 31 | */ 32 | enum FilterOptions : unsigned int 33 | { 34 | NoOptions = 0, /**< Represents empty option set */ 35 | AdditiveProcessNoise = 1 << 0, /**< Forces additive process noise */ 36 | AdditiveObsrvNoise = 1 << 1, /**< Forces additive observation noise */ 37 | FactorizeParams = 1 << 2 /**< Specifies param. state factorization */ 38 | }; 39 | 40 | enum ModelOptions : bool 41 | { 42 | IsNotAdaptive = 0, 43 | IsAdaptive = 1 44 | }; 45 | 46 | /** 47 | * \ingroup meta 48 | * Represents any number of available options as defined in #FilterOptions 49 | */ 50 | template struct Options { enum : signed int { Value = T }; }; 51 | 52 | /** 53 | * \ingroup meta 54 | * Combines multiple options into one. This allows to specify the options 55 | * listed in an arbitrary number 56 | */ 57 | template struct CombineOptions; 58 | 59 | /** 60 | * \internal 61 | * \ingroup meta 62 | * 63 | * Combines the first option within the option pack with the previous one. 64 | */ 65 | template 66 | struct CombineOptions : CombineOptions { }; 67 | 68 | /** 69 | * \internal 70 | * \ingroup meta 71 | * 72 | * Terminal case CombineOptions 73 | */ 74 | template 75 | struct CombineOptions { typedef fl::Options Options; }; 76 | 77 | /** 78 | * \internal 79 | * \ingroup meta 80 | * 81 | * No Option case 82 | */ 83 | template <> 84 | struct CombineOptions<> { typedef fl::Options Options; }; 85 | 86 | template 87 | struct MakeOptions : CombineOptions { }; 88 | 89 | } 90 | 91 | 92 | -------------------------------------------------------------------------------- /include/fl/util/meta/size_deduction.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file size_deduction.hpp 16 | * \date Febuary 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | 25 | namespace fl 26 | { 27 | 28 | /** 29 | * \ingroup meta 30 | * 31 | * \tparam Sizes Variadic argument containing a list of sizes. Each size 32 | * is a result of a constexpr or a const of a signed integral 33 | * type. Unknown sizes are represented by -1 or Eigen::Dynamic. 34 | * 35 | * 36 | * Joins the sizes within the \c Sizes argument list if all sizes are 37 | * non-dynamic. That is, all sizes are greater -1 (\c Eigen::Dynamic). If one of 38 | * the passed sizes is \c Eigen::Dynamic, the total size collapses to 39 | * \c Eigen::Dynamic. 40 | */ 41 | template struct JoinSizes; 42 | 43 | /** 44 | * \internal 45 | * \ingroup meta 46 | * \copydoc JoinSizes 47 | * 48 | * \tparam Head Head of the \c Sizes list of the previous recursive call 49 | * 50 | * Variadic counting recursion 51 | */ 52 | template struct JoinSizes 53 | { 54 | enum : signed int 55 | { 56 | Value = IsFixed() && IsFixed::Size>() 57 | ? Head + JoinSizes::Size 58 | : Eigen::Dynamic, 59 | Size = Value 60 | }; 61 | }; 62 | 63 | /** 64 | * \internal 65 | * \ingroup meta 66 | * 67 | * Terminal specialization of JoinSizes<...> 68 | */ 69 | template <> struct JoinSizes<> { enum: signed int { Size = 0, Value = 0 }; } ; 70 | 71 | /** 72 | * \ingroup meta 73 | * 74 | * \brief Function form of JoinSizes for two sizes 75 | */ 76 | inline constexpr int join_sizes(int a, int b) 77 | { 78 | return (a > Eigen::Dynamic && b > Eigen::Dynamic) ? a + b : Eigen::Dynamic; 79 | } 80 | 81 | /** 82 | * \ingroup meta 83 | * 84 | * Computes the product of LocalSize times Factor. If one of the parameters is 85 | * set to Eigen::Dynamic, the factor size will collapse to Eigen::Dynbamic as 86 | * well. 87 | */ 88 | template struct ExpandSizes; 89 | 90 | /** 91 | * \internal 92 | * \ingroup meta 93 | */ 94 | template struct ExpandSizes 95 | { 96 | enum: signed int 97 | { 98 | Value = IsFixed() && IsFixed::Size>() 99 | ? Head * ExpandSizes::Size 100 | : Eigen::Dynamic, 101 | Size = Value 102 | }; 103 | }; 104 | 105 | /** 106 | * \internal 107 | * \ingroup meta 108 | * 109 | * Terminal specialization of ExpandSizes<...> 110 | */ 111 | template <> struct ExpandSizes<> { enum: signed int { Size = 1, Value = 1 }; } ; 112 | 113 | } 114 | 115 | 116 | -------------------------------------------------------------------------------- /include/fl/util/meta/type_sequence.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file type_sequence.hpp 16 | * \date Febuary 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | namespace fl 24 | { 25 | 26 | /** 27 | * \ingroup meta 28 | * 29 | * Provides access to the the first type element in the specified variadic list 30 | */ 31 | template struct FirstTypeIn; 32 | 33 | /** 34 | * \internal 35 | * \ingroup meta 36 | * 37 | * Implementation of FirstTypeIn 38 | */ 39 | template 40 | struct FirstTypeIn 41 | { 42 | typedef First Type; 43 | }; 44 | 45 | /** 46 | * \ingroup meta 47 | * 48 | * Meta type defined in terms of a sequence of types 49 | */ 50 | template 51 | struct TypeSequence 52 | { 53 | enum : signed int { Size = sizeof...(T) }; 54 | }; 55 | 56 | /** 57 | * \internal 58 | * \ingroup meta 59 | * 60 | * Empty TypeSequence 61 | */ 62 | template <> struct TypeSequence<> 63 | { 64 | enum : signed int { Size = -1 }; 65 | }; 66 | 67 | /** 68 | * \ingroup meta 69 | * 70 | * Creates a \c TypeSequence by expanding the specified \c Type \c Count 71 | * times. 72 | */ 73 | template 74 | struct CreateTypeSequence 75 | : CreateTypeSequence 76 | { }; 77 | 78 | /** 79 | * \internal 80 | * \ingroup meta 81 | * 82 | * Terminal type of CreateTypeSequence 83 | */ 84 | template 85 | struct CreateTypeSequence<1, Type, T...> 86 | : TypeSequence 87 | { 88 | typedef TypeSequence TypeSeq; 89 | }; 90 | 91 | /** 92 | * \internal 93 | * \ingroup meta 94 | * 95 | * Terminal type of CreateTypeSequence for Count = 0 resulting in an empty 96 | * type sequence 97 | */ 98 | template 99 | struct CreateTypeSequence<0, Type> 100 | : TypeSequence<> 101 | { 102 | typedef TypeSequence<> TypeSeq; 103 | }; 104 | 105 | /** 106 | * \ingroup meta 107 | * 108 | * Creates an empty \c TypeSequence<> for a \c Count = Eigen::Dynamic 109 | */ 110 | template 111 | struct CreateTypeSequence<-1, Type> 112 | : TypeSequence<> 113 | { }; 114 | 115 | 116 | } 117 | 118 | 119 | -------------------------------------------------------------------------------- /include/fl/util/profiling.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file profiling.hpp 16 | * \author Manuel Wuthrich (manuel.wuthrich@gmail.com) 17 | */ 18 | 19 | #pragma once 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | // profiling macros 29 | #define GET_TIME(time) {struct timeval profiling_time; gettimeofday(&profiling_time, NULL);\ 30 | time = (profiling_time.tv_sec * 1000000u + profiling_time.tv_usec) /1000000.;} 31 | #ifdef PROFILING_ON 32 | #define PRINT(object) std::cout << object; 33 | 34 | #define INIT_PROFILING struct timeval profiling_start_time, profiling_end_time;\ 35 | gettimeofday(&profiling_start_time, NULL); 36 | #define RESET gettimeofday(&profiling_start_time, NULL); 37 | #define MEASURE(text)\ 38 | gettimeofday(&profiling_end_time, NULL);\ 39 | std::cout << "time for " << text << " " \ 40 | << std::setprecision(9) << std::fixed\ 41 | << ((profiling_end_time.tv_sec - profiling_start_time.tv_sec) * 1000000u\ 42 | + profiling_end_time.tv_usec - profiling_start_time.tv_usec) /1000000. \ 43 | << " s" << std::endl; gettimeofday(&profiling_start_time, NULL); 44 | #define MEASURE_FLUSH(text)\ 45 | gettimeofday(&profiling_end_time, NULL);\ 46 | std::cout << "\r";\ 47 | std::cout.flush();\ 48 | std::cout << "time for " << text << " " \ 49 | << std::setprecision(9) << std::fixed\ 50 | << ((profiling_end_time.tv_sec - profiling_start_time.tv_sec) * 1000000u\ 51 | + profiling_end_time.tv_usec - profiling_start_time.tv_usec) /1000000. \ 52 | << " s"; gettimeofday(&profiling_start_time, NULL); 53 | #else 54 | #define PRINT(object) 55 | #define INIT_PROFILING 56 | #define RESET 57 | #define MEASURE(text) 58 | #endif 59 | 60 | #define PShape(mat) std::cout << #mat << " (" << mat.rows() << ", " << mat.cols() << ")" << "\n\n"; 61 | #define PV(mat) std::cout << #mat << "\n" << mat << "\n\n"; 62 | #define PVT(mat) std::cout << #mat << "\n" << mat.transpose() << "\n"; 63 | #define PF(flag) std::cout << #flag << ":=" << flag << "\n"; 64 | #define PInfo(text) std::cout << "Info: " << text << "\n"; 65 | 66 | #ifdef NDEBUG 67 | #define break_on_fail(expr) (static_cast (0)) 68 | #else 69 | 70 | namespace fl 71 | { 72 | namespace internal 73 | { 74 | __attribute__ ((always_inline)) inline void __break_on_fail( 75 | __const char *__assertion, 76 | __const char *__file, 77 | unsigned int __line, 78 | __const char *__function 79 | ) __THROW 80 | { 81 | std::cout << "fl::breakpoint: " << __assertion << " failed at" 82 | << __file << ":" 83 | << __line << std::endl 84 | << __function << " " 85 | << std::endl; 86 | 87 | raise(SIGTRAP); 88 | } 89 | } 90 | } 91 | #define break_on_fail(expr) \ 92 | ((expr) \ 93 | ? (static_cast (0)) \ 94 | : fl::internal::__break_on_fail ( \ 95 | __STRING(expr), \ 96 | __FILE__, \ 97 | __LINE__, \ 98 | __PRETTY_FUNCTION__)) 99 | #endif 100 | 101 | #define pass_or_die(expr) if (!(expr)) { std::cout << #expr << std::endl; exit(-1); } 102 | -------------------------------------------------------------------------------- /include/fl/util/random.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file random.hpp 16 | * \date 2014 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | * \author Manuel Wuthrich (manuel.wuthrich@gmail.com) 19 | */ 20 | 21 | #pragma once 22 | 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | /** 30 | * \brief global seed 31 | * 32 | * \ingroup random 33 | */ 34 | //#ifdef fl_USE_RANDOM_SEED 35 | // #define RANDOM_SEED (unsigned int) std::time(0) 36 | //#else 37 | // #define RANDOM_SEED 1 38 | //#endif 39 | 40 | #undef RANDOM_SEED 41 | #define RANDOM_SEED (unsigned int) std::time(0) 42 | 43 | namespace fl 44 | { 45 | 46 | /** 47 | * \ingroup random 48 | * 49 | * \brief Mersenne Twister specialization mt11213b \cite matsumoto1998mersenne 50 | * 51 | * mt11213b is slightly faster than mt19937 52 | */ 53 | typedef std::mersenne_twister_engine< 54 | uint32_t, 55 | 32, 351, 175, 19, 56 | 0xccab8ee7, 11, 57 | 0xffffffff, 7, 58 | 0x31b6ab00, 15, 59 | 0xffe50000, 17, 60 | 1812433253 > mt11213b; 61 | 62 | /** 63 | * \ingroup random 64 | * \brief A seed. If fl_USE_RANDOM_SEED was set true the seed is set to the 65 | * current time, otherwise, the seed will be 1. 66 | * 67 | */ 68 | inline unsigned int seed() 69 | { 70 | //#ifdef fl_USE_RANDOM_SEED 71 | // static unsigned int seed_inc = std::time(0); 72 | // return seed_inc++; 73 | //#else 74 | // return 1; 75 | //#endif 76 | 77 | static unsigned int seed_inc = std::time(0); 78 | return seed_inc++; 79 | } 80 | 81 | } 82 | 83 | 84 | -------------------------------------------------------------------------------- /include/fl/util/scalar_matrix.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file scalar_matrix.hpp 16 | * \date August 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | #include 25 | 26 | namespace fl 27 | { 28 | 29 | /** 30 | * \ingroup types 31 | * 32 | * Represents a matrix of the size 1x1. This Matrix can be used in the same way 33 | * as a scalar. 34 | */ 35 | class ScalarMatrix 36 | : public Eigen::Matrix 37 | { 38 | public: 39 | /** 40 | * \brief Default constructor. 41 | * Creates ScalarMatrix with no arguments. The initial value is zero by 42 | * default. 43 | */ 44 | ScalarMatrix() 45 | { 46 | this->data()[0] = Real(0); 47 | } 48 | 49 | /** 50 | * \brief Creates a ScalarMatrix with an initial value or converts a scalar 51 | * into a ScalarMatrix. 52 | * \param value initial value 53 | */ 54 | ScalarMatrix(Real value) 55 | { 56 | this->data()[0] = value; 57 | } 58 | 59 | /** 60 | * \brief Constructor copying the value of the ScalarMatrix \a other 61 | */ 62 | ScalarMatrix(const ScalarMatrix& other) 63 | { 64 | this->data()[0] = other.data()[0]; 65 | } 66 | 67 | /** 68 | * \brief Constructor copying the value of the expression \a other 69 | */ 70 | template 71 | ScalarMatrix(const Eigen::MatrixBase &other) 72 | { 73 | this->data()[0] = other(0); 74 | } 75 | 76 | /** 77 | * \brief operator Real() implicit typecast conversion of a ScalarMatrix 78 | * into a scalar 79 | */ 80 | operator Real() const 81 | { 82 | return Real(this->data()[0]); 83 | } 84 | 85 | /** 86 | * \brief operator+= adds a scalar value to this matrix and assigns 87 | */ 88 | void operator+=(Real value) 89 | { 90 | this->data()[0] += value; 91 | } 92 | 93 | /** 94 | * \brief operator-= subtracts a scalar value to this matrix and assigns 95 | */ 96 | void operator-=(Real value) 97 | { 98 | this->data()[0] -= value; 99 | } 100 | 101 | /** 102 | * \brief operator*= multiplies a scalar value with this this matrix and 103 | * assigns 104 | */ 105 | void operator*=(Real value) 106 | { 107 | this->data()[0] *= value; 108 | } 109 | 110 | /** 111 | * \brief operator/= divide a scalar value with this this matrix and 112 | * assigns 113 | */ 114 | void operator/=(Real value) 115 | { 116 | this->data()[0] /= value; 117 | } 118 | 119 | /** 120 | * \brief prefix operator ++ which increments the matrix value by 1. 121 | */ 122 | ScalarMatrix& operator++() 123 | { 124 | return (++this->data()[0], *this); 125 | } 126 | 127 | /** 128 | * \brief postfix operator ++ which increments the matrix value by 1. 129 | */ 130 | ScalarMatrix operator++(int) 131 | { 132 | auto r = ScalarMatrix(this->data()[0]++); // RVO 133 | return r; 134 | } 135 | 136 | /** 137 | * \brief prefix operator -- which decrements the matrix value by 1. 138 | */ 139 | ScalarMatrix& operator--() 140 | { 141 | return (--this->data()[0], *this); 142 | } 143 | 144 | /** 145 | * \brief postfix operator -- which decrements the matrix value by 1. 146 | */ 147 | ScalarMatrix operator--(int) 148 | { 149 | auto r = ScalarMatrix(this->data()[0]--); // RVO 150 | return r; 151 | } 152 | }; 153 | 154 | } 155 | 156 | 157 | -------------------------------------------------------------------------------- /include/fl/util/types.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file types.hpp 16 | * \date June 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #pragma once 21 | 22 | 23 | #include 24 | 25 | namespace fl 26 | { 27 | 28 | #if defined(fl_USE_FLOAT) 29 | typedef float FloatingPoint; 30 | #elif defined(fl_USE_DOUBLE) 31 | typedef double FloatingPoint; 32 | #elif defined(fl_USE_LONG_DOUBLE) 33 | typedef long double FloatingPoint; 34 | #else 35 | typedef double FloatingPoint; 36 | #endif 37 | 38 | /** 39 | * \ingroup types 40 | * \brief Common floating point type. The default type of Real is \c double. 41 | * This type is used throughout the entire ::fl library. 42 | * 43 | * In order or use other basic floating point types please compile with one of 44 | * the following defines: 45 | * 46 | * - \c FL_USE_FLOAT: defines fl::Real as \c float 47 | * - \c FL_USE_DOUBLE: defines fl::Real as \c double (default) 48 | * - \c FL_USE_LONG_DOUBLE: defines fl::Real as \c long double 49 | */ 50 | typedef FloatingPoint Real; 51 | 52 | typedef Eigen::Matrix Vector1d; 53 | 54 | /** 55 | * \ingroup types 56 | */ 57 | template struct Additive 58 | { 59 | typedef Model_ Model; 60 | }; 61 | 62 | /** 63 | * \ingroup types 64 | */ 65 | template struct AdditiveUncorrelated 66 | { 67 | typedef Model_ Model; 68 | }; 69 | 70 | /** 71 | * \ingroup types 72 | */ 73 | template struct NonAdditive 74 | { 75 | typedef Model_ Model; 76 | }; 77 | 78 | /** 79 | * \internal 80 | */ 81 | namespace internal 82 | { 83 | 84 | /** 85 | * \internal 86 | * \ingroup types 87 | * 88 | * \brief Linear model type identifier 89 | */ 90 | struct LinearModelType { }; 91 | 92 | /** 93 | * \internal 94 | * \ingroup types 95 | * 96 | * \brief Observation model type identifier 97 | */ 98 | struct SensorType { }; 99 | 100 | /** 101 | * \internal 102 | * \ingroup types 103 | * 104 | * \brief Process model type identifier 105 | */ 106 | struct TransitionType { }; 107 | 108 | /** 109 | * \internal 110 | * \ingroup types 111 | * 112 | * \brief Adaptive model type identifier 113 | */ 114 | struct AdaptiveModelType { }; 115 | 116 | /** 117 | * \internal 118 | * \ingroup types 119 | * 120 | * \brief Represents the base type of any model with additive noise term 121 | * \f$ x_{t+1} = f(x_t) + v_t\f$ while \f$v_t\f$ is the additive noise. 122 | */ 123 | struct AdditiveNoiseModelType { }; 124 | 125 | /** 126 | * \internal 127 | * \ingroup types 128 | * 129 | * \brief Represents the base type of any model with additive uncorrelated 130 | * Gaussian white noise term in \f$ x_{t+1} = f(x_t) + v_t\f$ while \f$v_t\f$ is 131 | * the additive noise with \f$v_t \sim {\cal N}(v_t; 0, Q_t)\f$. Here, the 132 | * covariance matrix has a diagonal form \f$Q_t = \text{diag}(q_1, q_2, \ldots, 133 | * q_n)\f$ and \f$n\f$ is the dimension of \f$v_t \in \mathbb{R}^n\f$. 134 | */ 135 | struct AdditiveUncorrelatedNoiseModelType { }; 136 | 137 | /** 138 | * \internal 139 | * \ingroup types 140 | * 141 | * \brief Represents the base type of any model with non-additive noise term 142 | * \f$ x_{t+1} = f(x_t, v_t) \f$ while \f$v_t\f$ is the additive noise. 143 | */ 144 | struct NonAdditiveNoiseModelType { }; 145 | 146 | /** 147 | * \internal 148 | * \ingroup types 149 | * 150 | * \brief Base type of every iid JointSensor 151 | */ 152 | struct JointSensorIidType { }; 153 | 154 | 155 | /** 156 | * \internal 157 | * \ingroup types 158 | * 159 | * \brief Base type of every id JointSensor 160 | */ 161 | struct JointSensorIdType { }; 162 | 163 | } 164 | 165 | } 166 | 167 | 168 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fl 4 | 0.1.2 5 | The filtering_library package 6 | 7 | Jan Issac 8 | 9 | MIT 10 | 11 | http://github.com/filtering-library 12 | 13 | Jan Issac 14 | Manuel Wuethrich 15 | 16 | catkin 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /templates/filter.template.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 3 | * 4 | * 5 | */ 6 | 7 | /** 8 | * \file FILTER_FILE_NAME.hpp 9 | * \date 10 | * \author 11 | */ 12 | 13 | #ifndef FL__FILTER__YOUR_FILTER_NAME_HPP 14 | #define FL__FILTER__YOUR_FILTER_NAME_HPP 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | namespace fl 21 | { 22 | 23 | // Filter Forward declaration 24 | class YOUR_FILTER_NAME; 25 | 26 | // YOUR_FILTER_NAME Traits declaration 27 | /** 28 | * \internal 29 | * \brief ... 30 | * 31 | */ 32 | template <> 33 | struct Traits< YOUR_FILTER_NAME > 34 | { 35 | /* Required types */ 36 | 37 | typedef std::shared_ptr< YOUR_FILTER_NAME > Ptr; 38 | 39 | /* 40 | * Define your State and Input if not provided somewhere 41 | * E.g. 42 | * 43 | * typedef Eigen::Matrix State; 44 | * typedef Eigen::Matrix Input; 45 | * 46 | * or if provided by the process model 47 | * 48 | * typedef typename Traits::State State; 49 | * typedef typename Traits::Input Input; 50 | * 51 | * given a ProcessModel typedef in here 52 | */ 53 | typedef YOUR_STATE_TYPE State; 54 | typedef YOUR_INPUT_TYPE Input; 55 | 56 | /* 57 | * Define your Observation if not provided somewhere 58 | * E.g. 59 | * 60 | * typedef Eigen::Matrix Observation; 61 | * 62 | * or if provided by the process model 63 | * 64 | * typedef typename Traits::Observation Observation; 65 | * 66 | * given a ObservationModel typedef in here 67 | */ 68 | typedef YOUR_OBSERVATION_TYPE Observation; 69 | 70 | /* 71 | * Define the distribution type required by your filter. 72 | * Note that the distribution must implement the fl::Moments interface. 73 | * 74 | * A common used distribution is the fl::Gaussian. 75 | * 76 | * typedef ff::Gaussian StateDistribution; 77 | */ 78 | typedef YOUR_FILTER_DISTRIBUTION StateDistribution; 79 | }; 80 | 81 | 82 | /** 83 | * \ingroup filter 84 | * 85 | * \brief ... 86 | * 87 | * \details ... 88 | */ 89 | template <> 90 | class YOUR_FILTER_NAME 91 | : public FilterInterface< YOUR_FILTER_NAME > 92 | { 93 | public: 94 | typedef YOUR_FILTER_NAME This; 95 | 96 | // make the Traits types available within this scope 97 | typedef typename Traits::State State; 98 | typedef typename Traits::Input Input; 99 | typedef typename Traits::Observation Observation; 100 | typedef typename Traits::StateDistribution StateDistribution; 101 | 102 | 103 | // == implement the interface procedures ============================== // 104 | 105 | virtual void predict(double delta_time, 106 | const Input& input, 107 | const StateDistribution& prior_dist, 108 | StateDistribution& predicted_dist) 109 | { 110 | // here goes your filter prediction implementation 111 | } 112 | 113 | virtual void update(const Observation& observation, 114 | const StateDistribution& predicted_dist, 115 | StateDistribution& posterior_dist) 116 | { 117 | // here goes your filter update implementation 118 | } 119 | 120 | virtual void predict_and_update(double delta_time, 121 | const Input& input, 122 | const Observation& observation, 123 | const StateDistribution& prior_dist, 124 | StateDistribution& posterior_dist) 125 | { 126 | // this is the easiest solution 127 | predict(delta_time, input, prior_dist, posterior_dist); 128 | update(observation, posterior_dist, posterior_dist); 129 | 130 | // however, it might be far more efficient if you provide an 131 | // implementation which exploits the all in-one-place structure 132 | } 133 | 134 | // ... 135 | }; 136 | } 137 | 138 | #endif -------------------------------------------------------------------------------- /test/exception/exception_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \date 2014 16 | * \author Jan Issac (jan.issac@gmail.com) 17 | * Max-Planck-Institute for Intelligent Systems, University of Southern California 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | TEST(Exception, create) 26 | { 27 | struct FirstWeight 28 | { 29 | double w; 30 | std::string name; 31 | }; 32 | 33 | typedef fl::PointSet, -1> SigmaPointGaussian; 34 | SigmaPointGaussian sigmas(1); 35 | 36 | try 37 | { 38 | sigmas.point(0, Eigen::Matrix::Random(), {1.23, 1.24}); 39 | sigmas.point(1, Eigen::Matrix::Random(), {1.23, 1.24}); 40 | sigmas.point(2, Eigen::Matrix::Random(), {1.23, 1.24}); 41 | } 42 | catch(fl::OutOfBoundsException& e) { } 43 | catch(...) 44 | { 45 | ADD_FAILURE(); 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /test/filter_interface/filter_interface_stubs.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \date 2014 16 | * \author Jan Issac (jan.issac@gmail.com) 17 | * Max-Planck-Institute for Intelligent Systems, 18 | * University of Southern California 19 | */ 20 | 21 | #include 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | /* 30 | * FilterForFun forward declaration 31 | */ 32 | class FilterForFun; 33 | 34 | namespace fl 35 | { 36 | /* 37 | * FilterForFun Traits 38 | */ 39 | template <> struct Traits 40 | { 41 | typedef double State; 42 | typedef double Input; 43 | typedef double Obsrv; 44 | typedef double Belief; 45 | }; 46 | } 47 | 48 | /* 49 | * FilterForFun Stub Filter 50 | */ 51 | class FilterForFun: 52 | public fl::FilterInterface 53 | { 54 | public: 55 | typedef FilterForFun This; 56 | 57 | typedef typename fl::Traits::State State; 58 | typedef typename fl::Traits::Input Input; 59 | typedef typename fl::Traits::Obsrv Obsrv; 60 | typedef typename fl::Traits::Belief Belief; 61 | 62 | virtual void predict(const Belief& prior_belief, 63 | const Input& input, 64 | Belief& predicted_belief) override 65 | { 66 | predicted_belief = (prior_belief * 2); 67 | } 68 | 69 | virtual void update(const Belief& predicted_belief, 70 | const Obsrv& observation, 71 | Belief& posterior_belief) override 72 | { 73 | posterior_belief = (predicted_belief + observation) / 2.; 74 | } 75 | 76 | std::string name() const override 77 | { 78 | return "FilterForMoreFun"; 79 | } 80 | 81 | std::string description() const override 82 | { 83 | return "FilterForMoreFun"; 84 | } 85 | }; 86 | 87 | 88 | // == FilterForMoreFun Stub Filter ========================================== // 89 | 90 | /* 91 | * FilterForMoreFun forward declaration 92 | */ 93 | template class FilterForMoreFun; 94 | 95 | namespace fl 96 | { 97 | /* 98 | * FilterForMoreFun Traits 99 | */ 100 | template 101 | struct Traits> 102 | { 103 | typedef double State; 104 | typedef double Input; 105 | typedef double Obsrv; 106 | typedef double Belief; 107 | }; 108 | } 109 | 110 | /* 111 | * FilterForMoreFun Stub Filter 112 | */ 113 | template 114 | class FilterForMoreFun: 115 | public fl::FilterInterface> 116 | { 117 | public: 118 | typedef FilterForMoreFun This; 119 | 120 | typedef typename fl::Traits::State State; 121 | typedef typename fl::Traits::Input Input; 122 | typedef typename fl::Traits::Obsrv Obsrv; 123 | typedef typename fl::Traits::Belief Belief; 124 | 125 | void predict(const Belief& prior_belief, 126 | const Input& input, 127 | Belief& predicted_belief) override 128 | { 129 | predicted_belief = (prior_belief * 3); 130 | } 131 | 132 | void update(const Belief& predicted_belief, 133 | const Obsrv& observation, 134 | Belief& posterior_belief) override 135 | { 136 | posterior_belief = (predicted_belief + observation) / 3.; 137 | } 138 | 139 | std::string name() const override 140 | { 141 | return "FilterForMoreFun"; 142 | } 143 | 144 | std::string description() const override 145 | { 146 | return "FilterForMoreFun"; 147 | } 148 | }; 149 | -------------------------------------------------------------------------------- /test/filter_interface/filter_interface_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file filter_interface_test.cpp 16 | * \date October 2014 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | 24 | #include 25 | 26 | #include "filter_interface_stubs.hpp" 27 | 28 | /* 29 | * Generic FilterContext 30 | */ 31 | template 32 | class FilterContext 33 | { 34 | public: 35 | typedef FilterAlgorithm Filter; 36 | 37 | FilterContext(Filter _filter) 38 | : filter_(_filter), 39 | dist_(1.), 40 | u_(0.), 41 | y_(2.) 42 | { 43 | } 44 | 45 | void predict() 46 | { 47 | filter_.predict(dist_, u_, dist_); 48 | } 49 | 50 | void update() 51 | { 52 | filter_.update(dist_, y_, dist_); 53 | } 54 | 55 | Filter& filter() 56 | { 57 | return filter_; 58 | } 59 | 60 | Filter filter_; 61 | typename FilterAlgorithm::Belief dist_; 62 | typename FilterAlgorithm::Input u_; 63 | typename FilterAlgorithm::Obsrv y_; 64 | }; 65 | 66 | TEST(FilterInterface, NonTemplatedFilter) 67 | { 68 | auto filter_context = FilterContext(FilterForFun()); 69 | 70 | // predict pre-condition 71 | EXPECT_DOUBLE_EQ(filter_context.dist_, 1.); 72 | EXPECT_DOUBLE_EQ(filter_context.y_, 2.); 73 | 74 | EXPECT_NO_THROW(filter_context.predict()); 75 | 76 | // predict post-condition 77 | EXPECT_DOUBLE_EQ(filter_context.dist_, 2.); 78 | EXPECT_DOUBLE_EQ(filter_context.y_, 2.); 79 | 80 | EXPECT_NO_THROW(filter_context.update()); 81 | 82 | // update post-condition 83 | EXPECT_DOUBLE_EQ(filter_context.dist_, (2. + 2.)/2. ); 84 | EXPECT_DOUBLE_EQ(filter_context.y_, 2.); 85 | } 86 | 87 | TEST(FilterInterface, TemplatedFilter) 88 | { 89 | typedef FilterForMoreFun FilterAlgo; 90 | 91 | auto filter_context = FilterContext(FilterAlgo()); 92 | 93 | // predict pre-condition 94 | EXPECT_DOUBLE_EQ(filter_context.dist_, 1.); 95 | EXPECT_DOUBLE_EQ(filter_context.y_, 2.); 96 | 97 | EXPECT_NO_THROW(filter_context.predict()); 98 | 99 | // predict post-condition 100 | EXPECT_DOUBLE_EQ(filter_context.dist_, 3.); 101 | EXPECT_DOUBLE_EQ(filter_context.y_, 2.); 102 | 103 | EXPECT_NO_THROW(filter_context.update()); 104 | 105 | // update post-condition 106 | EXPECT_DOUBLE_EQ(filter_context.dist_, (3. + 2.)/3. ); 107 | EXPECT_DOUBLE_EQ(filter_context.y_, 2.); 108 | } 109 | -------------------------------------------------------------------------------- /test/gaussian_filter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}) 3 | 4 | include(gaussian_filter.cmake) 5 | include(sigma_point_quadrature.cmake) 6 | -------------------------------------------------------------------------------- /test/gaussian_filter/gaussian_filter.cmake: -------------------------------------------------------------------------------- 1 | 2 | # add_gaussian_filter_vs_x tests # 3 | function(add_gaussian_filter_test 4 | Name 5 | Type 6 | StateDim 7 | InputDim 8 | ObsrvDim 9 | ModelType 10 | TransitionNoiseType 11 | SensorNoiseType) 12 | 13 | set(TEST_FILE ${Name}) 14 | 15 | set(TEST_FILE_SUFFIX "${Type}_${StateDim}_${InputDim}_${ObsrvDim}_${ModelType}_${TransitionNoiseType}_${SensorNoiseType}") 16 | 17 | configure_file( 18 | ${CMAKE_CURRENT_SOURCE_DIR}/${TEST_FILE}.cpp.in 19 | ${CMAKE_CURRENT_BINARY_DIR}/${TEST_FILE}_${TEST_FILE_SUFFIX}.cpp @ONLY) 20 | 21 | fl_add_test( 22 | NAME ${TEST_FILE}_${TEST_FILE_SUFFIX} 23 | SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/${TEST_FILE}.hpp 24 | ${CMAKE_CURRENT_SOURCE_DIR}/gaussian_filter_linear_vs_x_test_suite.hpp 25 | ${CMAKE_CURRENT_BINARY_DIR}/${TEST_FILE}_${TEST_FILE_SUFFIX}.cpp) 26 | endfunction() 27 | 28 | set(CurrentTest "gaussian_filter_linear_vs_unscented_kalman_filter_test") 29 | 30 | add_gaussian_filter_test(${CurrentTest} StaticTest 2 1 2 GaussianModel NonAdditive NonAdditive) 31 | add_gaussian_filter_test(${CurrentTest} StaticTest 20 2 2 GaussianModel NonAdditive NonAdditive) 32 | add_gaussian_filter_test(${CurrentTest} StaticTest 20 4 20 GaussianModel NonAdditive NonAdditive) 33 | add_gaussian_filter_test(${CurrentTest} StaticTest 2 4 20 GaussianModel NonAdditive NonAdditive) 34 | 35 | add_gaussian_filter_test(${CurrentTest} DynamicTest 2 1 2 GaussianModel NonAdditive NonAdditive) 36 | add_gaussian_filter_test(${CurrentTest} DynamicTest 20 4 2 GaussianModel NonAdditive NonAdditive) 37 | add_gaussian_filter_test(${CurrentTest} DynamicTest 20 4 20 GaussianModel NonAdditive NonAdditive) 38 | 39 | add_gaussian_filter_test(${CurrentTest} StaticTest 2 1 2 GaussianModel Additive Additive) 40 | add_gaussian_filter_test(${CurrentTest} StaticTest 2 2 2 GaussianModel Additive Additive) 41 | add_gaussian_filter_test(${CurrentTest} StaticTest 20 4 2 GaussianModel Additive Additive) 42 | 43 | add_gaussian_filter_test(${CurrentTest} StaticTest 3 4 3 GaussianModel Additive Additive) 44 | add_gaussian_filter_test(${CurrentTest} StaticTest 20 4 20 GaussianModel Additive Additive) 45 | add_gaussian_filter_test(${CurrentTest} StaticTest 2 4 20 GaussianModel Additive Additive) 46 | 47 | add_gaussian_filter_test(${CurrentTest} DynamicTest 20 4 2 GaussianModel Additive Additive) 48 | add_gaussian_filter_test(${CurrentTest} DynamicTest 20 4 20 GaussianModel Additive Additive) 49 | add_gaussian_filter_test(${CurrentTest} DynamicTest 2 4 20 GaussianModel Additive Additive) 50 | 51 | add_gaussian_filter_test(${CurrentTest} StaticTest 20 4 2 DecorrelatedGaussianModel NonAdditive NonAdditive) 52 | add_gaussian_filter_test(${CurrentTest} StaticTest 20 4 200 DecorrelatedGaussianModel NonAdditive AdditiveUncorrelated) 53 | add_gaussian_filter_test(${CurrentTest} StaticTest 2 4 200 DecorrelatedGaussianModel NonAdditive NonAdditive) 54 | 55 | add_gaussian_filter_test(${CurrentTest} DynamicTest 20 4 2 DecorrelatedGaussianModel NonAdditive NonAdditive) 56 | add_gaussian_filter_test(${CurrentTest} DynamicTest 20 4 20 DecorrelatedGaussianModel NonAdditive NonAdditive) 57 | add_gaussian_filter_test(${CurrentTest} DynamicTest 20 4 200 DecorrelatedGaussianModel NonAdditive AdditiveUncorrelated) 58 | add_gaussian_filter_test(${CurrentTest} DynamicTest 2 4 200 DecorrelatedGaussianModel NonAdditive AdditiveUncorrelated) 59 | 60 | add_gaussian_filter_test(${CurrentTest} StaticTest 3 4 2 DecorrelatedGaussianModel Additive AdditiveUncorrelated) 61 | add_gaussian_filter_test(${CurrentTest} StaticTest 10 4 2 DecorrelatedGaussianModel Additive AdditiveUncorrelated) 62 | 63 | add_gaussian_filter_test(${CurrentTest} DynamicTest 4 4 4 DecorrelatedGaussianModel Additive AdditiveUncorrelated) 64 | add_gaussian_filter_test(${CurrentTest} DynamicTest 20 4 200 DecorrelatedGaussianModel Additive AdditiveUncorrelated) 65 | add_gaussian_filter_test(${CurrentTest} DynamicTest 2 4 200 DecorrelatedGaussianModel Additive AdditiveUncorrelated) 66 | -------------------------------------------------------------------------------- /test/gaussian_filter/gaussian_filter_linear_vs_unscented_kalman_filter_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the FL library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com) 6 | * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com) 7 | * 8 | * Max-Planck Institute for Intelligent Systems, AMD Lab 9 | * University of Southern California, CLMC Lab 10 | * 11 | * This Source Code Form is subject to the terms of the MIT License (MIT). 12 | * A copy of the license can be found in the LICENSE file distributed with this 13 | * source code. 14 | */ 15 | 16 | /** 17 | * \file gaussian_filter_linear_vs_unscented_kalman_filter_test.cpp 18 | * \date July 2015 19 | * \author Jan Issac (jan.issac@gmail.com) 20 | */ 21 | 22 | #include 23 | #include "../typecast.hpp" 24 | 25 | #include 26 | 27 | #include "gaussian_filter_linear_vs_x_test_suite.hpp" 28 | #include 29 | #include 30 | 31 | using namespace fl; 32 | 33 | template < 34 | int StateDimension, 35 | int InputDimension, 36 | int ObsrvDimension, 37 | int ModelKey 38 | > 39 | struct UkfTestConfig 40 | { 41 | enum : signed int 42 | { 43 | StateDim = StateDimension, 44 | InputDim = InputDimension, 45 | ObsrvDim = ObsrvDimension 46 | }; 47 | 48 | enum : signed int { SelectedModel = ModelKey }; 49 | 50 | template 51 | struct FilterDefinition 52 | { 53 | typedef GaussianFilter< 54 | StateTransitionModel, 55 | ObservationModel, 56 | SigmaPointQuadrature/*, 57 | SigmaPointPredictPolicy< 58 | SigmaPointQuadrature, 59 | typename UseAs>::Type>, 60 | SigmaPointUpdatePolicy< 61 | SigmaPointQuadrature, 62 | typename UseAs>::Type>*/ 63 | > Type; 64 | }; 65 | 66 | template 67 | static typename FilterDefinition::Type create_filter(F&& f, H&& h) 68 | { 69 | return typename FilterDefinition::Type( 70 | f, h, UnscentedQuadrature()); 71 | } 72 | }; 73 | 74 | typedef ::testing::Types< 75 | StaticTest>, 76 | StaticTest>, 77 | StaticTest>, 78 | StaticTest>, 79 | 80 | DynamicTest>, 81 | DynamicTest>, 82 | DynamicTest>, 83 | DynamicTest>, 84 | 85 | StaticTest>, 86 | StaticTest>, 87 | StaticTest>, 88 | StaticTest>, 89 | StaticTest>, 90 | 91 | DynamicTest>, 92 | DynamicTest>, 93 | DynamicTest>, 94 | DynamicTest>, 95 | DynamicTest> 96 | > TestTypes; 97 | 98 | INSTANTIATE_TYPED_TEST_CASE_P(GaussianFilterLinearVUkfTest, 99 | GaussianFilterLinearVsXTest, 100 | TestTypes); 101 | -------------------------------------------------------------------------------- /test/gaussian_filter/gaussian_filter_linear_vs_unscented_kalman_filter_test.cpp.in: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file gaussian_filter_linear_vs_unscented_kalman_filter_test.hpp 16 | * \date July 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | #include "gaussian_filter_linear_vs_unscented_kalman_filter_test.hpp" 24 | 25 | typedef ::testing::Types< 26 | fl::@Type@< 27 | UkfTestConfig< 28 | @StateDim@, 29 | @InputDim@, 30 | @ObsrvDim@, 31 | @ModelType@, 32 | fl::@TransitionNoiseType@, 33 | fl::@SensorNoiseType@ 34 | > 35 | > 36 | > TestTypes; 37 | 38 | INSTANTIATE_TYPED_TEST_CASE_P( 39 | GaussianFilterLinearVUkf_@Type@_@StateDim@_@InputDim@_@ObsrvDim@_@ModelType@_@TransitionNoiseType@_@SensorNoiseType@, 40 | GaussianFilterLinearVsXTest, 41 | TestTypes); 42 | -------------------------------------------------------------------------------- /test/gaussian_filter/gaussian_filter_linear_vs_unscented_kalman_filter_test.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file gaussian_filter_linear_vs_unscented_kalman_filter_test.cpp 16 | * \date July 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #include "../typecast.hpp" 28 | #include "gaussian_filter_linear_vs_x_test_suite.hpp" 29 | 30 | template < 31 | int StateDimension, 32 | int InputDimension, 33 | int ObsrvDimension, 34 | int ModelKey, 35 | template class TransitionNoiseType, 36 | template class SensorNoiseType 37 | > 38 | struct UkfTestConfig 39 | { 40 | enum : signed int 41 | { 42 | StateDim = StateDimension, 43 | InputDim = InputDimension, 44 | ObsrvDim = ObsrvDimension 45 | }; 46 | 47 | enum : signed int { SelectedModel = ModelKey }; 48 | 49 | template 50 | struct FilterDefinition 51 | { 52 | typedef fl::GaussianFilter< 53 | Transition, 54 | Sensor, 55 | fl::UnscentedQuadrature, 56 | fl::SigmaPointPredictPolicy< 57 | fl::UnscentedQuadrature, 58 | typename fl::UseAs< 59 | TransitionNoiseType 60 | >::Type>, 61 | fl::SigmaPointUpdatePolicy< 62 | fl::UnscentedQuadrature, 63 | typename fl::UseAs< 64 | SensorNoiseType 65 | >::Type> 66 | > Type; 67 | }; 68 | 69 | template 70 | static typename FilterDefinition::Type create_filter(F&& f, H&& h) 71 | { 72 | return typename FilterDefinition::Type( 73 | f, h, fl::UnscentedQuadrature()); 74 | } 75 | }; 76 | -------------------------------------------------------------------------------- /test/gaussian_filter/kalman_filter_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file gaussian_filter_kf_test.cpp 16 | * \date Febuary 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | #include "../typecast.hpp" 22 | 23 | #include 24 | 25 | #include "gaussian_filter_test_suite.hpp" 26 | 27 | #include 28 | 29 | template < 30 | int StateDimension, 31 | int InputDimension, 32 | int ObsrvDimension, 33 | int FilterIterations = 100 34 | > 35 | struct KalmanFilterTestConfiguration 36 | { 37 | enum: signed int 38 | { 39 | StateDim = StateDimension, 40 | InputDim = InputDimension, 41 | ObsrvDim = ObsrvDimension, 42 | Iterations = FilterIterations 43 | }; 44 | 45 | template 46 | struct FilterDefinition 47 | { 48 | typedef fl::GaussianFilter< 49 | typename ModelFactory::LinearTransition, 50 | typename ModelFactory::LinearObservation 51 | > Type; 52 | }; 53 | 54 | template 55 | static typename FilterDefinition::Type 56 | create_filter(ModelFactory&& factory) 57 | { 58 | return typename FilterDefinition::Type( 59 | factory.create_linear_state_model(), 60 | factory.create_sensor()); 61 | } 62 | }; 63 | 64 | typedef ::testing::Types< 65 | fl::StaticTest>, 66 | fl::StaticTest>, 67 | fl::StaticTest>, 68 | 69 | fl::DynamicTest>, 70 | fl::DynamicTest>, 71 | fl::DynamicTest> 72 | > TestTypes; 73 | 74 | INSTANTIATE_TYPED_TEST_CASE_P(KalmanFilterTest, 75 | GaussianFilterTest, 76 | TestTypes); 77 | -------------------------------------------------------------------------------- /test/gaussian_filter/multi_sensor_gaussian_filter_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file multi_sensor_gaussian_filter_test.cpp 16 | * \date August 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | #include "../typecast.hpp" 22 | 23 | #include 24 | 25 | #include "gaussian_filter_test_suite.hpp" 26 | #include 27 | #include 28 | #include 29 | 30 | using namespace fl; 31 | 32 | template < 33 | int StateDimension, 34 | int InputDimension, 35 | int ObsrvDimension, 36 | int Count, // Local observation model count 37 | int FilterIterations 38 | > 39 | struct MultiSensorGfTestConfiguration 40 | { 41 | enum : signed int 42 | { 43 | StateDim = StateDimension, 44 | InputDim = InputDimension, 45 | ObsrvDim = ObsrvDimension, 46 | Iterations = FilterIterations 47 | }; 48 | 49 | template 50 | struct FilterDefinition 51 | { 52 | enum : signed int 53 | { 54 | // compile time size (positive for static and -1 for dynamic) 55 | Size = ExpandSizes::Value 56 | }; 57 | 58 | typedef typename ModelFactory::LinearObservation LocalSensor; 59 | typedef JointSensor< 60 | MultipleOf 61 | > JointSensor; 62 | 63 | typedef UnscentedQuadrature Quadrature; 64 | 65 | typedef MultiSensorGaussianFilter< 66 | typename ModelFactory::LinearTransition, 67 | JointSensor, 68 | Quadrature 69 | > Type; 70 | }; 71 | 72 | template 73 | static typename FilterDefinition::Type 74 | create_filter(ModelFactory&& factory) 75 | { 76 | typedef typename 77 | FilterDefinition::JointSensor JointSensor; 78 | 79 | return typename FilterDefinition::Type( 80 | factory.create_linear_state_model(), 81 | JointSensor(factory.create_sensor(), Count), 82 | UnscentedQuadrature()); 83 | } 84 | }; 85 | 86 | typedef ::testing::Types< 87 | StaticTest> 88 | > TestTypes; 89 | 90 | INSTANTIATE_TYPED_TEST_CASE_P(MultiSensorGaussianFilterTest, 91 | GaussianFilterTest, 92 | TestTypes); 93 | 94 | -------------------------------------------------------------------------------- /test/gaussian_filter/robust_gaussian_filter_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file robust_gaussian_filter_test.cpp 16 | * \date August 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | 24 | #include "../typecast.hpp" 25 | #include "gaussian_filter_test_suite.hpp" 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | using namespace fl; 34 | 35 | template < 36 | int StateDimension, 37 | int InputDimension, 38 | int ObsrvDimension, 39 | int FilterIterations 40 | > 41 | struct RobutGaussianFilterTestConfiguration 42 | { 43 | enum : signed int 44 | { 45 | StateDim = StateDimension, 46 | InputDim = InputDimension, 47 | ObsrvDim = ObsrvDimension, 48 | Iterations = FilterIterations 49 | }; 50 | 51 | template 52 | struct FilterDefinition 53 | { 54 | typedef typename ModelFactory::LinearObservation::Obsrv Obsrv; 55 | typedef typename ModelFactory::LinearObservation::State State; 56 | 57 | typedef fl::LinearCauchySensor CauchyModel; 58 | 59 | typedef fl::BodyTailSensor< 60 | typename ModelFactory::LinearObservation, 61 | CauchyModel 62 | > BodyTailSensor; 63 | 64 | typedef UnscentedQuadrature Quadrature; 65 | // typedef fl::SigmaPointQuadrature< 66 | // fl::MonteCarloTransform< 67 | // fl::ConstantPointCountPolicy<1000>>> Quadrature; 68 | 69 | typedef RobustGaussianFilter< 70 | typename ModelFactory::LinearTransition, 71 | BodyTailSensor, 72 | Quadrature 73 | > Type; 74 | }; 75 | 76 | template 77 | static typename FilterDefinition::Type 78 | create_filter(ModelFactory&& factory) 79 | { 80 | typedef FilterDefinition Definition; 81 | typedef typename Definition::Type Filter; 82 | typedef typename Definition::CauchyModel CauchyModel; 83 | typedef typename Definition::BodyTailSensor BodyTailSensor; 84 | 85 | auto body_model = factory.create_sensor(); 86 | auto tail_model = CauchyModel(); 87 | tail_model.noise_covariance(tail_model.noise_covariance() * 10.); 88 | 89 | return Filter( 90 | factory.create_linear_state_model(), 91 | BodyTailSensor(body_model, tail_model, 0.1), 92 | typename Definition::Quadrature()); 93 | } 94 | }; 95 | 96 | 97 | typedef ::testing::Types< 98 | StaticTest> 99 | > TestTypes; 100 | 101 | INSTANTIATE_TYPED_TEST_CASE_P(RobustGaussianFilterTest, 102 | GaussianFilterTest, 103 | TestTypes); 104 | -------------------------------------------------------------------------------- /test/gaussian_filter/sigma_point_quadrature.cmake: -------------------------------------------------------------------------------- 1 | 2 | # add_gaussian_filter_vs_x tests # 3 | function(add_sigma_point_quadrature_test 4 | Name 5 | Type 6 | DimA 7 | DimB 8 | Transform) 9 | 10 | set(TEST_FILE ${Name}) 11 | 12 | set(TEST_FILE_SUFFIX "${Type}_${DimA}_${DimB}_${Transform}") 13 | 14 | configure_file( 15 | ${CMAKE_CURRENT_SOURCE_DIR}/${TEST_FILE}.cpp.in 16 | ${CMAKE_CURRENT_BINARY_DIR}/${TEST_FILE}_${TEST_FILE_SUFFIX}.cpp @ONLY) 17 | 18 | fl_add_test( 19 | NAME ${TEST_FILE}_${TEST_FILE_SUFFIX} 20 | SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/${TEST_FILE}.hpp 21 | ${CMAKE_CURRENT_BINARY_DIR}/${TEST_FILE}_${TEST_FILE_SUFFIX}.cpp) 22 | endfunction() 23 | 24 | set(CurrentTest "sigma_point_quadrature_test") 25 | 26 | add_sigma_point_quadrature_test(${CurrentTest} StaticTest 1 1 Unscented) 27 | add_sigma_point_quadrature_test(${CurrentTest} StaticTest 2 2 Unscented) 28 | add_sigma_point_quadrature_test(${CurrentTest} StaticTest 3 3 Unscented) 29 | add_sigma_point_quadrature_test(${CurrentTest} StaticTest 6 3 Unscented) 30 | add_sigma_point_quadrature_test(${CurrentTest} StaticTest 3 6 Unscented) 31 | add_sigma_point_quadrature_test(${CurrentTest} StaticTest 24 3 Unscented) 32 | add_sigma_point_quadrature_test(${CurrentTest} StaticTest 50 50 Unscented) 33 | 34 | add_sigma_point_quadrature_test(${CurrentTest} DynamicTest 1 1 Unscented) 35 | add_sigma_point_quadrature_test(${CurrentTest} DynamicTest 2 2 Unscented) 36 | add_sigma_point_quadrature_test(${CurrentTest} DynamicTest 3 3 Unscented) 37 | add_sigma_point_quadrature_test(${CurrentTest} DynamicTest 6 3 Unscented) 38 | add_sigma_point_quadrature_test(${CurrentTest} DynamicTest 3 6 Unscented) 39 | add_sigma_point_quadrature_test(${CurrentTest} DynamicTest 24 3 Unscented) 40 | add_sigma_point_quadrature_test(${CurrentTest} DynamicTest 50 50 Unscented) 41 | 42 | add_sigma_point_quadrature_test(${CurrentTest} StaticTest 1 1 MonteCarlo) 43 | add_sigma_point_quadrature_test(${CurrentTest} StaticTest 2 2 MonteCarlo) 44 | add_sigma_point_quadrature_test(${CurrentTest} StaticTest 3 3 MonteCarlo) 45 | add_sigma_point_quadrature_test(${CurrentTest} StaticTest 6 3 MonteCarlo) 46 | add_sigma_point_quadrature_test(${CurrentTest} StaticTest 3 6 MonteCarlo) 47 | add_sigma_point_quadrature_test(${CurrentTest} StaticTest 24 3 MonteCarlo) 48 | add_sigma_point_quadrature_test(${CurrentTest} StaticTest 50 50 MonteCarlo) 49 | 50 | add_sigma_point_quadrature_test(${CurrentTest} DynamicTest 1 1 MonteCarlo) 51 | add_sigma_point_quadrature_test(${CurrentTest} DynamicTest 2 2 MonteCarlo) 52 | add_sigma_point_quadrature_test(${CurrentTest} DynamicTest 3 3 MonteCarlo) 53 | add_sigma_point_quadrature_test(${CurrentTest} DynamicTest 6 3 MonteCarlo) 54 | add_sigma_point_quadrature_test(${CurrentTest} DynamicTest 3 6 MonteCarlo) 55 | add_sigma_point_quadrature_test(${CurrentTest} DynamicTest 24 3 MonteCarlo) 56 | add_sigma_point_quadrature_test(${CurrentTest} DynamicTest 50 50 MonteCarlo) 57 | -------------------------------------------------------------------------------- /test/gaussian_filter/sigma_point_quadrature_test.cpp.in: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file sigma_point_quadrature_test.cpp.in 16 | * \date August 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | 22 | #include "sigma_point_quadrature_test.hpp" 23 | 24 | // typedef the names of available transform for the test cases 25 | 26 | typedef fl::UnscentedTransform UnscentedTransform; 27 | 28 | typedef fl::MonteCarloTransform< 29 | fl::LinearPointCountPolicy<100> 30 | > MonteCarloTransform; 31 | 32 | 33 | typedef ::testing::Types< 34 | fl::@Type@< 35 | TestConfiguration< 36 | @DimA@, 37 | @DimB@, 38 | @Transform@Transform 39 | > 40 | > 41 | > TestTypes; 42 | 43 | INSTANTIATE_TYPED_TEST_CASE_P( 44 | @Type@_@DimA@_@DimB@_@Transform@, 45 | SigmaPointQuadratureTests, 46 | TestTypes); 47 | -------------------------------------------------------------------------------- /test/gaussian_filter/unscented_kalman_filter_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file gaussian_filter_kf_test.cpp 16 | * \date Febuary 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | #include "../typecast.hpp" 22 | 23 | #include 24 | 25 | #include "gaussian_filter_test_suite.hpp" 26 | #include 27 | #include 28 | 29 | using namespace fl; 30 | 31 | template < 32 | int StateDimension, 33 | int InputDimension, 34 | int ObsrvDimension, 35 | int FilterIterations = 100 36 | > 37 | struct UnscentedKalmanFilterTestConfiguration 38 | { 39 | enum : signed int 40 | { 41 | StateDim = StateDimension, 42 | InputDim = InputDimension, 43 | ObsrvDim = ObsrvDimension, 44 | Iterations = FilterIterations 45 | }; 46 | 47 | template 48 | struct FilterDefinition 49 | { 50 | typedef UnscentedQuadrature Quadrature; 51 | 52 | typedef GaussianFilter< 53 | typename ModelFactory::LinearTransition, 54 | typename ModelFactory::LinearObservation, 55 | Quadrature 56 | > Type; 57 | }; 58 | 59 | template 60 | static typename FilterDefinition::Type 61 | create_filter(ModelFactory&& factory) 62 | { 63 | return typename FilterDefinition::Type( 64 | factory.create_linear_state_model(), 65 | factory.create_sensor(), 66 | UnscentedQuadrature()); 67 | } 68 | }; 69 | 70 | typedef ::testing::Types< 71 | StaticTest>, 72 | StaticTest>, 73 | StaticTest>, 74 | DynamicTest>, 75 | DynamicTest>, 76 | DynamicTest> 77 | > TestTypes; 78 | 79 | INSTANTIATE_TYPED_TEST_CASE_P(UnscentedKalmanFilterTest, 80 | GaussianFilterTest, 81 | TestTypes); 82 | -------------------------------------------------------------------------------- /test/gtest_main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the FL library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com) 6 | * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com) 7 | * 8 | * Max-Planck Institute for Intelligent Systems, AMD Lab 9 | * University of Southern California, CLMC Lab 10 | * 11 | * This Source Code Form is subject to the terms of the MIT License (MIT). 12 | * A copy of the license can be found in the LICENSE file distributed with this 13 | * source code. 14 | */ 15 | 16 | /** 17 | * \file gtest_main.hpp 18 | * \date May 2014 19 | * \author Jan Issac (jan.issac@gmail.com) 20 | */ 21 | #include 22 | 23 | int main(int argc, char **argv) 24 | { 25 | testing::InitGoogleTest(&argc, argv); 26 | 27 | return RUN_ALL_TESTS(); 28 | } 29 | -------------------------------------------------------------------------------- /test/model/binary_transition_density_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * @date 2015 16 | * @author Manuel Wuthrich (manuel.wuthrich@gmail.com) 17 | * Max-Planck-Institute for Intelligent Systems 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | fl::Real epsilon = 0.000000000001; 26 | fl::Real large_dt = 9999999999999.; 27 | 28 | TEST(binary_transition_density, unit_delta_time) 29 | { 30 | fl::Real p_1to1 = 0.6; 31 | fl::Real p_0to1 = 0.3; 32 | fl::BinaryTransitionDensity density(p_1to1, p_0to1); 33 | 34 | EXPECT_TRUE(std::fabs(density.probability(1,1,1.) - p_1to1) < epsilon); 35 | EXPECT_TRUE(std::fabs(density.probability(1,0,1.) - p_0to1) < epsilon); 36 | } 37 | 38 | TEST(binary_transition_density, zero_delta_time) 39 | { 40 | fl::BinaryTransitionDensity density(0.6, 0.3); 41 | 42 | EXPECT_TRUE(std::fabs(density.probability(1,1,0.) - 1.) < epsilon); 43 | EXPECT_TRUE(std::fabs(density.probability(0,0,0.) - 1.) < epsilon); 44 | 45 | EXPECT_TRUE(density.probability(1,0,0.) < epsilon); 46 | EXPECT_TRUE(density.probability(0,1,0.) < epsilon); 47 | } 48 | 49 | TEST(binary_transition_density, inifinite_delta_time) 50 | { 51 | fl::Real p_1to1 = 0.6; 52 | fl::Real p_0to1 = 0.3; 53 | fl::BinaryTransitionDensity density(p_1to1, p_0to1); 54 | 55 | // the limit for dt -> infinity 56 | fl::Real limit = p_0to1 / (1. - p_1to1 + p_0to1); 57 | 58 | EXPECT_TRUE(std::fabs(density.probability(1,0,large_dt) - limit) < epsilon); 59 | EXPECT_TRUE(std::fabs(density.probability(1,1,large_dt) - limit) < epsilon); 60 | } 61 | 62 | TEST(binary_transition_density, consistency) 63 | { 64 | fl::Real p_1to1 = 0.6; 65 | fl::Real p_0to1 = 0.3; 66 | fl::BinaryTransitionDensity density(p_1to1, p_0to1); 67 | 68 | fl::Real dt = 0.1; 69 | int N_steps = 10; 70 | fl::Real initial_p_1 = 0.5; 71 | 72 | fl::Real p_1 = initial_p_1; 73 | for(int i = 0; i < N_steps; i++) 74 | { 75 | // step-wise computation 76 | p_1 = density.probability(1, 1, dt) * p_1 77 | + density.probability(1, 0, dt) * (1.-p_1); 78 | 79 | // direct computation 80 | fl::Real p_1_ = 81 | density.probability(1, 1, (i+1) * dt) * initial_p_1 82 | + density.probability(1, 0, (i+1) * dt) * (1.-initial_p_1); 83 | 84 | 85 | EXPECT_TRUE(std::fabs(p_1 - p_1_) < epsilon); 86 | } 87 | } 88 | 89 | TEST(binary_transition_density, constant_system) 90 | { 91 | fl::Real p_1to1 = 1.; 92 | fl::Real p_0to1 = 0.; 93 | fl::BinaryTransitionDensity density(p_1to1, p_0to1); 94 | 95 | EXPECT_TRUE(std::fabs(density.probability(1,1,large_dt) - 1.) < epsilon); 96 | EXPECT_TRUE(std::fabs(density.probability(0,0,large_dt) - 1.) < epsilon); 97 | 98 | EXPECT_TRUE(density.probability(0,1,large_dt) < epsilon); 99 | EXPECT_TRUE(density.probability(1,0,large_dt) < epsilon); 100 | } 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | -------------------------------------------------------------------------------- /test/model/transition/orientation_transition_function_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file linear_transition_test.hpp 16 | * \date October 2014 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | 24 | #include 25 | -------------------------------------------------------------------------------- /test/typecast.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file typecast.cpp 16 | * \date June 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | #pragma once 26 | 27 | 28 | namespace fl 29 | { 30 | 31 | struct NoParameter { }; 32 | 33 | template 34 | struct DynamicTest { typedef Param Parameter; }; 35 | 36 | template 37 | struct StaticTest { typedef Param Parameter; }; 38 | 39 | template struct TestSize; 40 | 41 | template 42 | struct TestSize> 43 | { 44 | enum : signed int { Value = Size }; 45 | }; 46 | 47 | template 48 | struct TestSize> 49 | { 50 | enum : signed int { Value = Eigen::Dynamic }; 51 | }; 52 | 53 | template 54 | struct IntegerTypePair 55 | { 56 | enum : signed int { Key = Key_ }; 57 | typedef TypeName Type; 58 | }; 59 | 60 | template 61 | struct IntegerTypeMapImp 62 | : IntegerTypeMapImp 63 | { }; 64 | 65 | template 66 | struct IntegerTypeMapImp, Pairs...> 67 | { 68 | typedef TypeName Type; 69 | }; 70 | 71 | template 72 | struct IntegerTypeMap 73 | { 74 | template struct Select: IntegerTypeMapImp { }; 75 | }; 76 | 77 | } 78 | 79 | 80 | -------------------------------------------------------------------------------- /test/utils/descriptor_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file descriptor_test.hpp 16 | * \date July 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | 24 | template class Dummy; 25 | 26 | template <> 27 | class Dummy<> 28 | : public fl::Descriptor 29 | { 30 | public: 31 | virtual std::string name() const 32 | { 33 | return "Dummy<>"; 34 | } 35 | 36 | virtual std::string description() const 37 | { 38 | return "Dummy class implementing the fl::Descriptor"; 39 | } 40 | }; 41 | 42 | template 43 | class NestedDummy 44 | : public D 45 | { 46 | public: 47 | using D::indent; 48 | 49 | virtual std::string name() const 50 | { 51 | return "NestedDummy<" + indent(D::name()) + ">"; 52 | } 53 | 54 | virtual std::string description() const 55 | { 56 | return "NestedDummy class implementing the fl::Descriptor with" 57 | + indent(D::description()); 58 | } 59 | }; 60 | 61 | TEST(DescriptorTests, Dummy_name) 62 | { 63 | Dummy<> dummy; 64 | EXPECT_STREQ(dummy.name().c_str(), "Dummy<>"); 65 | } 66 | 67 | TEST(DescriptorTests, Dummy_description) 68 | { 69 | Dummy<> dummy; 70 | EXPECT_STREQ(dummy.description().c_str(), 71 | "Dummy class implementing the fl::Descriptor"); 72 | } 73 | 74 | TEST(DescriptorTests, Dummy_indent) 75 | { 76 | Dummy<> dummy; 77 | 78 | EXPECT_STREQ(dummy.indent(dummy.description()).c_str() , 79 | "\n Dummy class implementing the fl::Descriptor"); 80 | } 81 | 82 | TEST(DescriptorTests, NestedDummy_name) 83 | { 84 | NestedDummy> dummy; 85 | EXPECT_STREQ(dummy.name().c_str(), "NestedDummy<\n Dummy<>>"); 86 | } 87 | 88 | TEST(DescriptorTests, NestedDummy_description) 89 | { 90 | NestedDummy> dummy; 91 | EXPECT_STREQ(dummy.description().c_str(), 92 | "NestedDummy class implementing the fl::Descriptor with" 93 | "\n Dummy class implementing the fl::Descriptor"); 94 | } 95 | 96 | TEST(DescriptorTests, NestedDummy_indent) 97 | { 98 | NestedDummy> dummy; 99 | 100 | EXPECT_STREQ(dummy.indent(dummy.description()).c_str() , 101 | "\n NestedDummy class implementing the fl::Descriptor with" 102 | "\n Dummy class implementing the fl::Descriptor"); 103 | } 104 | 105 | 106 | TEST(DescriptorTests, NestedNestedDummy_name) 107 | { 108 | NestedDummy>> dummy; 109 | 110 | EXPECT_STREQ(dummy.name().c_str(), 111 | "NestedDummy<\n NestedDummy<\n Dummy<>>>"); 112 | } 113 | 114 | TEST(DescriptorTests, NestedNestedDummy_description) 115 | { 116 | NestedDummy>> dummy; 117 | 118 | EXPECT_STREQ(dummy.description().c_str(), 119 | "NestedDummy class implementing the fl::Descriptor with" 120 | "\n NestedDummy class implementing the fl::Descriptor with" 121 | "\n Dummy class implementing the fl::Descriptor"); 122 | } 123 | -------------------------------------------------------------------------------- /test/utils/linear_algebra_is_diagonal_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file linear_algebra_is_diagonal_test.cpp 16 | * \date July 15 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | static constexpr int Dim = 10; 26 | 27 | template 28 | void check_is_diagonal() 29 | { 30 | Eigen::Matrix m; 31 | 32 | m.resize(Dim, Dim); 33 | m.setRandom(); 34 | 35 | EXPECT_FALSE(fl::is_diagonal(m)); 36 | 37 | m = m.diagonal().asDiagonal(); 38 | EXPECT_TRUE(fl::is_diagonal(m)); 39 | 40 | m.setIdentity(); 41 | m *= 3.; 42 | EXPECT_TRUE(fl::is_diagonal(m)); 43 | 44 | m(0, Dim - 1) = 2; 45 | EXPECT_FALSE(fl::is_diagonal(m)); 46 | } 47 | 48 | TEST(LinearAlgebra, is_diagonal_fixed_size) 49 | { 50 | check_is_diagonal(); 51 | } 52 | 53 | TEST(LinearAlgebra, is_diagonal_dynamic_size) 54 | { 55 | check_is_diagonal(); 56 | } 57 | -------------------------------------------------------------------------------- /test/utils/linear_algebra_square_root_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file linear_algebra_square_root_test.cpp 16 | * \date July 15 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | static constexpr int N = 50; 26 | 27 | template 28 | void check_square_root() 29 | { 30 | Matrix M; 31 | Matrix L; 32 | 33 | M.resize(N, N); 34 | M.setRandom(); 35 | M *= M.transpose(); 36 | fl::square_root(M, L ); 37 | 38 | EXPECT_TRUE(fl::are_similar(L*L.transpose(), M)); 39 | } 40 | 41 | TEST(LinearAlgebra, square_root_fixed_size) 42 | { 43 | check_square_root>(); 44 | } 45 | 46 | TEST(LinearAlgebra, square_root_dynamic_size) 47 | { 48 | check_square_root>(); 49 | } 50 | -------------------------------------------------------------------------------- /test/utils/meta_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file meta_test.hpp 16 | * \date Januray 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | using namespace fl; 31 | 32 | TEST(MetaTests, ExpandSizes) 33 | { 34 | EXPECT_EQ((ExpandSizes<-1, 10>::Size), -1); 35 | EXPECT_EQ((ExpandSizes<10, -1>::Size), -1); 36 | EXPECT_EQ((ExpandSizes<-1, -1>::Size), -1); 37 | 38 | EXPECT_EQ((ExpandSizes<1, 1>::Size), 1); 39 | EXPECT_EQ((ExpandSizes<1, 10>::Size), 10); 40 | 41 | EXPECT_EQ((ExpandSizes<2, 10>::Size), 20); 42 | EXPECT_EQ((ExpandSizes<3, 5>::Size), 15); 43 | } 44 | 45 | TEST(MetaTests, JoinSizes_single_param) 46 | { 47 | EXPECT_EQ((JoinSizes<0>::Size), 0); 48 | EXPECT_EQ((JoinSizes<-1>::Size), -1); 49 | EXPECT_EQ((JoinSizes<1>::Size), 1); 50 | EXPECT_EQ((JoinSizes<2>::Size), 2); 51 | } 52 | 53 | TEST(MetaTests, JoinSizes_two_param) 54 | { 55 | EXPECT_EQ((JoinSizes<-1, -1>::Size), -1); 56 | EXPECT_EQ((JoinSizes<-1, 1>::Size), -1); 57 | EXPECT_EQ((JoinSizes< 1, -1>::Size), -1); 58 | EXPECT_EQ((JoinSizes< 1, 1>::Size), 2); 59 | 60 | // EXPECT_EQ((JoinSizes<-1, 0>::Size), -1); // should be rejected 61 | // EXPECT_EQ((JoinSizes< 1, 0>::Size), 2); // should be rejected 62 | } 63 | 64 | TEST(MetaTests, JoinSizes_multiple_param) 65 | { 66 | EXPECT_EQ((JoinSizes<-1, -1, 1, 2, -1>::Size), -1); 67 | EXPECT_EQ((JoinSizes< 1, -1, 1, 2, -1>::Size), -1); 68 | EXPECT_EQ((JoinSizes< 1, 1, 1, 2, -1>::Size), -1); 69 | EXPECT_EQ((JoinSizes<-1, 1, 1, 2, -1>::Size), -1); 70 | EXPECT_EQ((JoinSizes< 1, -1, 1, 2, -1>::Size), -1); 71 | EXPECT_EQ((JoinSizes<1, 1, 1, 1, 1>::Size), 5); 72 | EXPECT_EQ((JoinSizes<1, 2, 3, 4, 5>::Size), 5*6/2); 73 | } 74 | -------------------------------------------------------------------------------- /test/utils/options_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file options_test.hpp 16 | * \date Febuary 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | 24 | using namespace fl; 25 | 26 | TEST(MetaOptionsTests, CombineOptions_NoOptions) 27 | { 28 | EXPECT_EQ(CombineOptions<>::Options::Value, NoOptions); 29 | EXPECT_EQ(CombineOptions::Options::Value, NoOptions); 30 | 31 | EXPECT_EQ( 32 | CombineOptions<>::Options::Value, Options::Value); 33 | EXPECT_EQ( 34 | CombineOptions::Options::Value, Options::Value); 35 | } 36 | 37 | 38 | TEST(MetaOptionsTests, CombineOptions_single_argument) 39 | { 40 | EXPECT_EQ( 41 | CombineOptions::Options::Value, 42 | AdditiveProcessNoise 43 | ); 44 | 45 | EXPECT_EQ( 46 | CombineOptions::Options::Value, 47 | Options::Value 48 | ); 49 | } 50 | 51 | TEST(MetaOptionsTests, CombineOptions_two_argument) 52 | { 53 | EXPECT_EQ( 54 | (CombineOptions::Options::Value), 56 | AdditiveProcessNoise | AdditiveObsrvNoise 57 | ); 58 | 59 | EXPECT_EQ( 60 | (CombineOptions::Options::Value), 62 | Options::Value 63 | ); 64 | } 65 | 66 | TEST(MetaOptionsTests, CombineOptions_three_argument) 67 | { 68 | EXPECT_EQ( 69 | (CombineOptions< 70 | AdditiveProcessNoise, 71 | AdditiveObsrvNoise, 72 | FactorizeParams>::Options::Value), 73 | AdditiveProcessNoise | AdditiveObsrvNoise | FactorizeParams 74 | ); 75 | 76 | EXPECT_EQ( 77 | (CombineOptions< 78 | AdditiveProcessNoise, 79 | AdditiveObsrvNoise, 80 | FactorizeParams>::Options::Value), 81 | Options< 82 | AdditiveProcessNoise | 83 | AdditiveObsrvNoise | 84 | FactorizeParams>::Value 85 | ); 86 | } 87 | -------------------------------------------------------------------------------- /test/utils/special_functions_normal_to_uniform_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file special_functions_normal_to_uniform_test.cpp 16 | * \date August 2015 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | TEST(SpecialFunctions, normal_to_uniform) 26 | { 27 | fl::StandardGaussian normal; 28 | 29 | for(int i = 0; i < 100000; ++i) 30 | { 31 | fl::Real r = normal.sample(); 32 | EXPECT_GT(fl::normal_to_uniform(r), 0.0); 33 | EXPECT_LT(fl::normal_to_uniform(r), 1.0); 34 | } 35 | } 36 | 37 | TEST(SpecialFunctions, mean) 38 | { 39 | fl::StandardGaussian normal; 40 | 41 | fl::Real mean = 0; 42 | int iterations = 1e6; 43 | 44 | for(int i = 0; i < iterations; ++i) 45 | { 46 | mean += fl::normal_to_uniform(normal.sample()); 47 | } 48 | 49 | mean /= fl::Real(iterations); 50 | 51 | EXPECT_GT(mean, 0.4); 52 | EXPECT_LT(mean, 0.6); 53 | } 54 | -------------------------------------------------------------------------------- /test/utils/traits_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This is part of the fl library, a C++ Bayesian filtering library 3 | * (https://github.com/filtering-library) 4 | * 5 | * Copyright (c) 2015 Max Planck Society, 6 | * Autonomous Motion Department, 7 | * Institute for Intelligent Systems 8 | * 9 | * This Source Code Form is subject to the terms of the MIT License (MIT). 10 | * A copy of the license can be found in the LICENSE file distributed with this 11 | * source code. 12 | */ 13 | 14 | /** 15 | * \file traits_test.hpp 16 | * \date October 2014 17 | * \author Jan Issac (jan.issac@gmail.com) 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | TEST(TraitsTests, is_dynamic) 30 | { 31 | EXPECT_TRUE(fl::IsDynamic()); 32 | EXPECT_FALSE(fl::IsDynamic<10>()); 33 | } 34 | 35 | TEST(TraitsTests, is_fixed) 36 | { 37 | EXPECT_FALSE(fl::IsFixed()); 38 | EXPECT_TRUE(fl::IsFixed<10>()); 39 | } 40 | 41 | TEST(TraitsTests, DimensionOf_dynamic) 42 | { 43 | EXPECT_EQ(fl::DimensionOf(), 0); 44 | EXPECT_EQ(fl::DimensionOf(), 0); 45 | 46 | typedef Eigen::Matrix PartiallyDynamicMatrix; 47 | EXPECT_EQ(fl::DimensionOf(), 0); 48 | } 49 | 50 | TEST(TraitsTests, DimensionOf_fixed) 51 | { 52 | EXPECT_EQ(fl::DimensionOf(), 3); 53 | EXPECT_EQ(fl::DimensionOf(), 4); 54 | 55 | typedef Eigen::Matrix FixedMatrix; 56 | EXPECT_EQ(fl::DimensionOf(), 10); 57 | 58 | typedef Eigen::Matrix FixedVector; 59 | EXPECT_EQ(fl::DimensionOf(), 10); 60 | } 61 | --------------------------------------------------------------------------------