├── .github └── workflows │ └── ci.yml ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cfg ├── GravityCompensation.params ├── KalmanFilter.params ├── LowPassFilter.params ├── MovingMean.params └── Threshold.params ├── filters_plugin.xml ├── include └── iirob_filters │ ├── gravity_compensation.h │ ├── kalman_filter.h │ ├── low_pass_filter.h │ ├── moving_mean_filter.h │ └── threshold_filter.h ├── package.xml ├── src ├── gravity_compensation.cpp ├── kalman_filter.cpp ├── low_pass_filter.cpp ├── moving_mean_filter.cpp └── threshold_filter.cpp └── test ├── how_to_run_tests.md ├── test_kalman.cpp ├── test_kalman.launch └── test_kalman.yaml /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: Test iirob_filters 2 | on: 3 | pull_request: 4 | push: 5 | branches: 6 | - melodic 7 | schedule: 8 | # Run every morning to detect flakiness and broken dependencies 9 | - cron: '17 8 * * *' 10 | 11 | jobs: 12 | ci_melodic_release: 13 | name: Melodic against release repository 14 | strategy: 15 | matrix: 16 | env: 17 | - {ROS_DISTRO: melodic, ROS_REPO: main} 18 | runs-on: ubuntu-latest 19 | steps: 20 | - uses: actions/checkout@v1 21 | - uses: 'ros-industrial/industrial_ci@master' 22 | env: ${{matrix.env}} 23 | 24 | ci_kinetic_release: 25 | name: Kinetic against release repository 26 | strategy: 27 | matrix: 28 | env: 29 | - {ROS_DISTRO: kinetic, ROS_REPO: main} 30 | runs-on: ubuntu-latest 31 | steps: 32 | - uses: actions/checkout@v1 33 | - uses: 'ros-industrial/industrial_ci@master' 34 | env: ${{matrix.env}} 35 | 36 | 37 | ci_melodic_testing: 38 | name: Melodic against testing repository 39 | strategy: 40 | matrix: 41 | env: 42 | - {ROS_DISTRO: melodic, ROS_REPO: testing} 43 | runs-on: ubuntu-latest 44 | steps: 45 | - uses: actions/checkout@v1 46 | - uses: 'ros-industrial/industrial_ci@master' 47 | env: ${{matrix.env}} 48 | 49 | ci_kinetic_testing: 50 | name: Kinetic against testing repository 51 | strategy: 52 | matrix: 53 | env: 54 | - {ROS_DISTRO: kinetic, ROS_REPO: testing} 55 | runs-on: ubuntu-latest 56 | steps: 57 | - uses: actions/checkout@v1 58 | - uses: 'ros-industrial/industrial_ci@master' 59 | env: ${{matrix.env}} 60 | 61 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package iirob_filters 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | * Add ros_geometry_msgs dependency for build 8 | * Use GitHub actions as CI 9 | 10 | 0.9.1 (2020-02-22) 11 | ------------------ 12 | * Repository changes 13 | 14 | 0.9.0 (2020-02-22) 15 | ------------------ 16 | * Add melodic travis build (`#31 `_) 17 | * Add melodic travis build to kinetic-branch 18 | * Removed support for indigo 19 | * Use local namespace for parameters 20 | * Contributors: Denis Štogl 21 | 22 | 0.8.2 (2020-02-17) 23 | ------------------ 24 | * Added chages to sequential Kalman Filter (`#30 `_) 25 | * added handling if At is empty (`#29 `_) 26 | * version 0.8.1 27 | * generated changelog 28 | * Contributors: Denis Štogl, GDwag, Gilbert Groten, Daniel Azanov 29 | 30 | 0.8.1 (2018-12-13) 31 | ------------------ 32 | * Revert use of doTransform for Wrenches (#24) 33 | * Update README.md (#25) 34 | * Update README.md (#23) 35 | * Use doTransform for Wrenches (#22) 36 | * Use doTransform for Wrenches 37 | * Corrected package.xml repository website and moved from Eigen to Eigen3 in CMakeLists.txt 38 | * Added author 39 | * Added Kalman filter implementation only for target tracking. (#14) 40 | * Added Kalman filter implementation with static and in time changing matrices. 41 | * Merge pull request #21 from KITrobotics/info_output 42 | Added info output for filters. Added world frame for gravity_compensa… 43 | * Added info output for filters. Added world frame for gravity_compensation and small refractoring. 44 | * Merge pull request #19 from KITrobotics/destogl-patch-1 45 | Update README.md 46 | * Update .travis.yml 47 | * Update .travis.yml 48 | * Update .travis.rosinstall 49 | * Update package.xml 50 | * Update CMakeLists.txt 51 | * Update README.md 52 | * Merge pull request #16 from iirob/gravity_output 53 | Added output for gravity compenation 54 | * Update gravity_compensation.h 55 | * Added output for gravity compenation 56 | * Merge pull request #13 from iirob/raw_with_fts 57 | Raw with fts 58 | * changes to work with double and wrench stamped 59 | * use rosparam handler 60 | * xMerge branch 'raw_with_fts' into me 61 | * added moving mean filter 62 | * Update gravity_compensation.h 63 | * Update gravity_compensation.h 64 | * filters working with wrench template and implementing interface from ros filters: need code review 65 | * changes to correctly work with filters plugin, code not reviewed 66 | * Merge pull request #10 from iirob/raw_with_fts 67 | Raw with fts 68 | * Update CMakeLists.txt 69 | * Filters are now matching to FilterBase interface 70 | * Merge branch 'raw_with_fts' of https://github.com/iirob/iirob_filters into raw_with_fts 71 | * changes on gravity compensation and threshold filter to match the FilterBase interface 72 | * Merge branch 'kinetic-devel' into raw_with_fts 73 | * Merge branch 'indigo-devel' into kinetic-devel 74 | * changes to avoid rosparam_handler error while compiling 75 | * changes to add pluginlib 76 | * Merge pull request #8 from iirob/raw_with_fts 77 | Raw with fts 2nd time 78 | * Merge branch 'indigo-devel' into raw_with_fts 79 | * Merge pull request #7 from iirob/raw_with_fts 80 | Raw with fts updated 81 | * Update .travis.rosinstall 82 | * Merge branch 'kinetic-devel' into raw_with_fts 83 | * small change 84 | * used changes from indigo-devel 85 | * adopted changes from indigo-devel 86 | * changes needed to work with rosparam_handler 87 | * Merge pull request #6 from iirob/raw_with_fts 88 | Raw with fts 89 | * Merge branch 'indigo-devel' into raw_with_fts 90 | * Merge pull request #5 from iirob/raw_with_fts 91 | Raw with fts 92 | * Update .travis.rosinstall 93 | * Added custom param_handler into travis 94 | * Merge branch 'raw_with_fts' of github.com:iirob/iirob_filters into raw_with_fts 95 | * Reverted to c++11 96 | * Reverted to c++11 97 | * compatibility changes 98 | * merged with kinetic-devel 99 | * ros params for filters working 100 | * Merge pull request #4 from iirob/library_rename_export 101 | Library rename export 102 | * Merge pull request #3 from iirob/library_rename_export 103 | Update CMakeLists.txt 104 | * Update CMakeLists.txt 105 | * Merge pull request #2 from iirob/add_install_space 106 | Added install space for iirob_filters 107 | * Merge pull request #1 from iirob/add_install_space 108 | Added install space for iirob_filters 109 | * Added install space for iirob_filters 110 | * Removed Eigen warning 111 | * Merge branch 'indigo-devel' into kinetic-devel 112 | * Removed kinetic test from indigo branch 113 | * Added dummy .rosinstall file 114 | * revert to trusty 115 | * Updated travis config to xenial for kinetic 116 | * Added readme file 117 | * Added tf2 dependecy 118 | * Added travis config 119 | * Update .travis.yml 120 | * Updated moving mean filter to correct correctly 121 | * Updated GravityCompensation filter to accept messages in any frame and return them in the same frame. Reduced transformations error output so that every 100th error is shown. 122 | * Renamed median to moving_mean and updated headers in files 123 | * fixed parameters in init 124 | * created 'init'-functions 125 | * update 126 | * Inital coping of files into filters 127 | * Contributors: Andreea Tulbure, Denis Štogl, IIROB Praktikum 1, Timo Leitritz, muritane 128 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(iirob_filters) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | cmake_modules 6 | dynamic_reconfigure 7 | eigen_conversions 8 | filters 9 | geometry_msgs 10 | roscpp 11 | rosparam_handler 12 | tf2_geometry_msgs 13 | tf2_ros 14 | ) 15 | 16 | find_package(Eigen3) 17 | if(NOT EIGEN3_FOUND) 18 | # Fallback to cmake_modules 19 | find_package(cmake_modules REQUIRED) 20 | find_package(Eigen REQUIRED) 21 | set(Eigen3_INCLUDE_DIRS ${Eigen_INCLUDE_DIRS}) 22 | set(Eigen3_LIBRARIES ${Eigen_LIBRARIES}) 23 | else() 24 | set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 25 | set(Eigen3_LIBRARIES ${EIGEN3_LIBRARIES}) 26 | endif() 27 | 28 | include(CheckCXXCompilerFlag) 29 | CHECK_CXX_COMPILER_FLAG("-std=c++11" Cpp11CompilerFlag) 30 | if (Cpp11CompilerFlag) 31 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 32 | 33 | endif() 34 | 35 | generate_ros_parameter_files( 36 | cfg/Threshold.params 37 | cfg/LowPassFilter.params 38 | cfg/GravityCompensation.params 39 | cfg/KalmanFilter.params 40 | cfg/MovingMean.params 41 | ) 42 | 43 | 44 | catkin_package( 45 | INCLUDE_DIRS include 46 | CATKIN_DEPENDS eigen_conversions filters geometry_msgs roscpp rosparam_handler 47 | DEPENDS Eigen3 48 | LIBRARIES ${PROJECT_NAME} 49 | ) 50 | 51 | ########### 52 | ## Build ## 53 | ########### 54 | 55 | include_directories( 56 | include 57 | ${catkin_INCLUDE_DIRS} 58 | ${Eigen3_INCLUDE_DIRS} 59 | ) 60 | 61 | add_library(${PROJECT_NAME} 62 | src/gravity_compensation.cpp 63 | src/low_pass_filter.cpp 64 | src/threshold_filter.cpp 65 | src/kalman_filter.cpp 66 | src/moving_mean_filter.cpp 67 | 68 | include/${PROJECT_NAME}/kalman_filter.h 69 | include/${PROJECT_NAME}/low_pass_filter.h 70 | include/${PROJECT_NAME}/gravity_compensation.h 71 | include/${PROJECT_NAME}/threshold_filter.h 72 | include/${PROJECT_NAME}/moving_mean_filter.h 73 | ) 74 | 75 | add_dependencies(${PROJECT_NAME} 76 | ${PROJECT_NAME}_gencfg # For dynamic_reconfigure 77 | ${PROJECT_NAME}_genparam # For rosparam_handler 78 | ) 79 | 80 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES}) 81 | 82 | ############# 83 | ## Install ## 84 | ############# 85 | 86 | ## Mark executables and/or libraries for installation 87 | install(TARGETS ${PROJECT_NAME} 88 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 89 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 90 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 91 | ) 92 | 93 | ## Mark cpp header files for installation 94 | install(DIRECTORY include/${PROJECT_NAME}/ 95 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 96 | FILES_MATCHING PATTERN "*.h" 97 | PATTERN ".svn" EXCLUDE 98 | ) 99 | install(FILES filters_plugin.xml 100 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 101 | ) 102 | 103 | ############# 104 | ## Testing ## 105 | ############# 106 | 107 | ## Add gtest based cpp test target and link libraries 108 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_iirob_filters.cpp) 109 | # if(TARGET ${PROJECT_NAME}-test) 110 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 111 | # endif() 112 | 113 | ## Add folders to be run by python nosetests 114 | # catkin_add_nosetests(test) 115 | 116 | if(CATKIN_ENABLE_TESTING) 117 | find_package(rostest REQUIRED) 118 | add_executable(kalman_test test/test_kalman.cpp) 119 | target_link_libraries(kalman_test ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${GTEST_LIBRARIES}) 120 | add_rostest(test/test_kalman.launch) 121 | 122 | if(TARGET tests) 123 | add_dependencies(tests kalman_test) 124 | endif() 125 | endif() 126 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | iirob_filters 2 | ============= 3 | 4 | ## ROS Distro Support 5 | * Kinetic 6 | * Melodic 7 | 8 | [![Build Status](https://github.com/KITrobotics/iirob_filters/workflows/Test%20iirob_filters/badge.svg?branch=melodic)](https://github.com/KITrobotics/iirob_filters/actions?query=branch%3Amelodic) 9 | [![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) 10 | -------------------------------------------------------------------------------- /cfg/GravityCompensation.params: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from rosparam_handler.parameter_generator_catkin import * 3 | gen = ParameterGenerator() 4 | 5 | # Add a Gravity Compensation group 6 | my_group = gen.add_group("GravityCompensation") 7 | my_group.add("world_frame", paramtype="std::string", description="refrence coordinate system", default="", configurable=True) 8 | my_group.add("sensor_frame", paramtype="std::string", description="sensor coordinate system", default="", configurable=True) 9 | my_group.add("CoG_x", paramtype="double", description="value", default=0, configurable=True) 10 | my_group.add("CoG_y", paramtype="double", description="value", default=0, configurable=True) 11 | my_group.add("CoG_z", paramtype="double", description="value", default=0, configurable=True) 12 | my_group.add("force", paramtype="double", description="value", default=0, configurable=True) 13 | 14 | #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) 15 | exit(gen.generate("iirob_filters", "iirob_filters", "GravityCompensation")) 16 | -------------------------------------------------------------------------------- /cfg/KalmanFilter.params: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from rosparam_handler.parameter_generator_catkin import * 3 | gen = ParameterGenerator() 4 | 5 | # Add a Kalman Filter group 6 | my_group = gen.add_group("KalmanFilter") 7 | my_group.add("dt", paramtype="double", description="time between two updates", default=0, configurable=True) 8 | my_group.add("n", paramtype="int", description="number of states", default=0, configurable=True) 9 | my_group.add("m", paramtype="int", description="number of measurements", default=0, configurable=True) 10 | my_group.add("A", paramtype="std::vector", description="System dynamics matrix(static part): nxn", default=[], configurable=False) 11 | my_group.add("At", paramtype="std::vector", description="System dynamics matrix(dynamic part): nxn", default=[], configurable=False) 12 | my_group.add("C", paramtype="std::vector", description="Output matrix: mxn", default=[], configurable=False) 13 | my_group.add("Q", paramtype="std::vector", description="Process noise covariance: nxn", default=[], configurable=False) 14 | my_group.add("Q_coeff", paramtype="std::vector", description="coefficients of the dynamic part of Q", default=[], configurable=False) 15 | my_group.add("Q_exponent", paramtype="std::vector", description="exponents of the time differnce", default=[], configurable=False) 16 | my_group.add("Q_variance", paramtype="double", description="variance of the process noise", default=0, configurable=False) 17 | my_group.add("R", paramtype="std::vector", description="Measurement noise covariance: mxm", default=[], configurable=False) 18 | my_group.add("P", paramtype="std::vector", description="Estimate error covariance: nxn", default=[], configurable=False) 19 | my_group.add("x0", paramtype="std::vector", description="start state vector", default=[], configurable=False) 20 | 21 | #my_group.add("dynamics", paramtype="bool", description="if dynamic state space", default=False, configurable=True) 22 | #my_group.add("t0", paramtype="double", description="start time", default=0, configurable=True) 23 | 24 | #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) 25 | exit(gen.generate("iirob_filters", "iirob_filters_kalman_filter", "KalmanFilter")) 26 | 27 | -------------------------------------------------------------------------------- /cfg/LowPassFilter.params: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from rosparam_handler.parameter_generator_catkin import * 3 | gen = ParameterGenerator() 4 | 5 | # Add a Lowpass Filter group 6 | my_group = gen.add_group("LowPassFilter") 7 | my_group.add("SamplingFrequency", paramtype="double", description="sampling frequency", default=0, configurable=True) 8 | my_group.add("DampingFrequency", paramtype="double", description="sampling frequency", default=0, configurable=True) 9 | my_group.add("DampingIntensity", paramtype="double", description="sampling frequency", default=0, configurable=True) 10 | my_group.add("divider", paramtype="int", description="divider", default=1, configurable=True) 11 | #gen.add("LowPassFilter", paramtype="std::map", description="A map parameter") 12 | 13 | #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) 14 | exit(gen.generate("iirob_filters", "iirob_filters", "LowPassFilter")) 15 | -------------------------------------------------------------------------------- /cfg/MovingMean.params: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from rosparam_handler.parameter_generator_catkin import * 3 | gen = ParameterGenerator() 4 | 5 | gen.add("divider", paramtype="int", description="mean divider", default=4, configurable=True) 6 | 7 | #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) 8 | exit(gen.generate("iirob_filters", "iirob_filters", "MovingMean")) 9 | -------------------------------------------------------------------------------- /cfg/Threshold.params: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from rosparam_handler.parameter_generator_catkin import * 3 | gen = ParameterGenerator() 4 | 5 | #Add a Threshold Filter group 6 | my_group = gen.add_group("ThresholdFilter") 7 | my_group.add("linear_threshold", paramtype="double", description="linear threshold", default=0, configurable=True) 8 | my_group.add("angular_threshold", paramtype="double", description="angular threshold", default=0, configurable=True) 9 | my_group.add("threshold", paramtype="double", description="threshold for double", default=0, configurable=True) 10 | 11 | #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) 12 | exit(gen.generate("iirob_filters", "iirob_filters", "Threshold")) 13 | -------------------------------------------------------------------------------- /filters_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This is a gravity compensator. 5 | 6 | 7 | 8 | 9 | This is a low pass filter. 10 | 11 | 12 | 13 | 14 | This is a threshold filter. 15 | 16 | 17 | 18 | 19 | This is a low pass filter working with geometry_msgs::WrenchStamped. 20 | 21 | 22 | 23 | 24 | This is a gravity compensator working with geometry_msgs::WrenchStamped. 25 | 26 | 27 | 28 | 29 | This is a threshold filter working with geometry_msgs::WrenchStamped. 30 | 31 | 32 | 34 | 35 | This is a low pass filter which works on a stream of vectors of doubles. 36 | 37 | 38 | 39 | 41 | 42 | This is a Kalman filter which works on a stream of vectors of doubles. 43 | 44 | 45 | 47 | 48 | This is a threshold filter which works on a stream of vectors of doubles. 49 | 50 | 51 | 53 | 54 | This is a moving mean which works with geometry_msgs::WrenchStamped 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /include/iirob_filters/gravity_compensation.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * 3 | * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group, 4 | * Institute for Anthropomatics and Robotics (IAR) - 5 | * Intelligent Process Control and Robotics (IPR), 6 | * Karlsruhe Institute of Technology (KIT) 7 | * 8 | * Author: Andreea Tulbure, email: andreea.tulbure@student.kit.edu 9 | * Denis Štogl, email: denis.stogl@kit.edu 10 | * Alexandar Pollmann 11 | * 12 | * Date of creation: 2015-2017 13 | * 14 | * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 15 | * 16 | * Redistribution and use in source and binary forms, 17 | * with or without modification, are permitted provided 18 | * that the following conditions are met: 19 | * 20 | * * Redistributions of source code must retain the above copyright 21 | * notice, this list of conditions and the following disclaimer. 22 | * * Redistributions in binary form must reproduce the above copyright 23 | * notice, this list of conditions and the following disclaimer in the 24 | * documentation and/or other materials provided with the distribution. 25 | * * Neither the name of the copyright holder nor the names of its 26 | * contributors may be used to endorse or promote products derived from 27 | * this software without specific prior written permission. 28 | * 29 | * This package is free software: you can redistribute it and/or modify 30 | * it under the terms of the GNU Lesser General Public License as published by 31 | * the Free Software Foundation, either version 3 of the License, or 32 | * (at your option) any later version. 33 | * 34 | * This package is distributed in the hope that it will be useful, 35 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 36 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 37 | * GNU Lesser General Public License for more details. 38 | * 39 | * You should have received a copy of the GNU Lesser General Public License 40 | * along with this package. If not, see . 41 | *****************************************************************************/ 42 | 43 | #ifndef IIROB_FILTERS_GRAVITY_COMPENSATION_H 44 | #define IIROB_FILTERS_GRAVITY_COMPENSATION_H 45 | #include 46 | #include 47 | #include 48 | 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | 61 | namespace iirob_filters{ 62 | 63 | template 64 | class GravityCompensator: public filters::FilterBase 65 | { 66 | public: 67 | GravityCompensator(); 68 | 69 | /** \brief Destructor to clean up 70 | */ 71 | ~GravityCompensator(); 72 | 73 | virtual bool configure(); 74 | 75 | /** \brief Update the filter and return the data seperately 76 | * \param data_in T array with length width 77 | * \param data_out T array with length width 78 | */ 79 | virtual bool update( const T & data_in, T& data_out); 80 | 81 | private: 82 | iirob_filters::GravityCompensationParameters params_; 83 | 84 | // Storage for Calibration Values 85 | geometry_msgs::Vector3Stamped cog_; // Center of Gravity Vector (wrt Sensor Frame) 86 | double force_z_; // Gravitational Force 87 | 88 | // Frames for Transformation of Gravity / CoG Vector 89 | std::string world_frame_; 90 | std::string sensor_frame_; 91 | 92 | // tf2 objects 93 | tf2_ros::Buffer *p_tf_Buffer_; 94 | tf2_ros::TransformListener *p_tf_Listener; 95 | geometry_msgs::TransformStamped transform_, transform_back_; 96 | 97 | uint _num_transform_errors; 98 | 99 | dynamic_reconfigure::Server reconfigCalibrationSrv_; // Dynamic reconfiguration service 100 | 101 | void reconfigureConfigurationRequest(iirob_filters::GravityCompensationConfig& config, uint32_t level); 102 | }; 103 | 104 | template 105 | GravityCompensator::GravityCompensator(): params_{ros::NodeHandle("~/GravityCompensation/params").getNamespace()} 106 | { 107 | reconfigCalibrationSrv_.setCallback(boost::bind(&GravityCompensator::reconfigureConfigurationRequest, this, _1, _2)); 108 | } 109 | 110 | template 111 | GravityCompensator::~GravityCompensator() 112 | { 113 | } 114 | 115 | template 116 | bool GravityCompensator::configure() 117 | { 118 | params_.fromParamServer(); 119 | if(params_.world_frame == " ") 120 | ROS_ERROR("GravityCompensator did not find param world_frame"); 121 | if(params_.sensor_frame == " ") 122 | ROS_DEBUG("GravityCompensator did not find param sensor_frame"); 123 | if(params_.CoG_x == 0) 124 | ROS_DEBUG("GravityCompensator did not find param CoG_x"); 125 | if(params_.CoG_y == 0) 126 | ROS_DEBUG("GravityCompensator did not find param CoG_y"); 127 | if(params_.CoG_z == 0) 128 | ROS_DEBUG("GravityCompensator did not find param CoG_z"); 129 | if(params_.force == 0) 130 | ROS_DEBUG("GravityCompensator did not find param force"); 131 | 132 | world_frame_ = params_.world_frame; 133 | sensor_frame_ = params_.sensor_frame; 134 | cog_.vector.x = params_.CoG_x; 135 | cog_.vector.y = params_.CoG_y; 136 | cog_.vector.z = params_.CoG_z; 137 | force_z_ = params_.force; 138 | 139 | ROS_INFO("Gravity Compensation Params: world_frame: %s; sensor_frame: %s; CoG_x:%f; CoG_y:%f; CoG_z:%f; force: %f." , world_frame_.c_str(), sensor_frame_.c_str(), 140 | cog_.vector.x,cog_. vector.y,cog_. vector.z,force_z_); 141 | 142 | p_tf_Buffer_ = new tf2_ros::Buffer; 143 | p_tf_Listener = new tf2_ros::TransformListener(*p_tf_Buffer_,true); 144 | _num_transform_errors = 0; 145 | 146 | return true; 147 | } 148 | template 149 | bool GravityCompensator::update(const T & data_in, T& data_out) 150 | { 151 | try 152 | { 153 | transform_ = p_tf_Buffer_->lookupTransform(world_frame_, data_in.header.frame_id, ros::Time(0)); 154 | transform_back_ = p_tf_Buffer_->lookupTransform(data_in.header.frame_id, world_frame_, ros::Time(0)); 155 | _num_transform_errors = 0; 156 | } 157 | catch (tf2::TransformException ex) 158 | { 159 | if (_num_transform_errors%100 == 0){ 160 | ROS_ERROR("%s", ex.what()); 161 | } 162 | _num_transform_errors++; 163 | } 164 | 165 | geometry_msgs::Vector3Stamped temp_force_transformed, temp_torque_transformed, temp_vector_in, temp_vector_out; 166 | 167 | temp_vector_in.vector = data_in.wrench.force; 168 | tf2::doTransform(temp_vector_in, temp_force_transformed, transform_); 169 | 170 | temp_vector_in.vector = data_in.wrench.torque; 171 | tf2::doTransform(temp_vector_in, temp_torque_transformed, transform_); 172 | 173 | // Transform CoG Vector 174 | geometry_msgs::Vector3Stamped cog_transformed; 175 | tf2::doTransform(cog_, cog_transformed, transform_); 176 | 177 | // Compensate for gravity force 178 | temp_force_transformed.vector.z += force_z_; 179 | // Compensation Values for torque result from Crossprod of cog Vector and (0 0 G) 180 | temp_torque_transformed.vector.x += (force_z_ * cog_transformed.vector.y); 181 | temp_torque_transformed.vector.y -= (force_z_ * cog_transformed.vector.x); 182 | 183 | // Copy Message and Compensate values for Gravity Force and Resulting Torque 184 | //geometry_msgs::WrenchStamped compensated_wrench; 185 | data_out = data_in; 186 | 187 | tf2::doTransform(temp_force_transformed, temp_vector_out, transform_back_); 188 | data_out.wrench.force = temp_vector_out.vector; 189 | 190 | tf2::doTransform(temp_torque_transformed, temp_vector_out, transform_back_); 191 | data_out.wrench.torque = temp_vector_out.vector; 192 | return true; 193 | } 194 | 195 | template 196 | void GravityCompensator::reconfigureConfigurationRequest(iirob_filters::GravityCompensationConfig& config, uint32_t level) 197 | { 198 | //params_.fromConfig(config); 199 | world_frame_ = params_.world_frame; 200 | sensor_frame_ = params_.sensor_frame; 201 | cog_.vector.x = params_.CoG_x; 202 | cog_.vector.y = params_.CoG_y; 203 | cog_.vector.z = params_.CoG_z; 204 | force_z_ = params_.force; 205 | 206 | 207 | }; 208 | 209 | } 210 | #endif 211 | 212 | -------------------------------------------------------------------------------- /include/iirob_filters/kalman_filter.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * 3 | * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group, 4 | * Institute for Anthropomatics and Robotics (IAR) - 5 | * Intelligent Process Control and Robotics (IPR), 6 | * Karlsruhe Institute of Technology (KIT) 7 | * 8 | * Author: Denis Štogl, email: denis.stogl@kit.edu 9 | * 10 | * Date of creation: 2016 11 | * 12 | * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 13 | * 14 | * Redistribution and use in source and binary forms, 15 | * with or without modification, are permitted provided 16 | * that the following conditions are met: 17 | * 18 | * * Redistributions of source code must retain the above copyright 19 | * notice, this list of conditions and the following disclaimer. 20 | * * Redistributions in binary form must reproduce the above copyright 21 | * notice, this list of conditions and the following disclaimer in the 22 | * documentation and/or other materials provided with the distribution. 23 | * * Neither the name of the copyright holder nor the names of its 24 | * contributors may be used to endorse or promote products derived from 25 | * this software without specific prior written permission. 26 | * 27 | * This package is free software: you can redistribute it and/or modify 28 | * it under the terms of the GNU Lesser General Public License as published by 29 | * the Free Software Foundation, either version 3 of the License, or 30 | * (at your option) any later version. 31 | * 32 | * This package is distributed in the hope that it will be useful, 33 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 34 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 35 | * GNU Lesser General Public License for more details. 36 | * 37 | * You should have received a copy of the GNU Lesser General Public License 38 | * along with this package. If not, see . 39 | *****************************************************************************/ 40 | 41 | #ifndef IIROB_FILTERS_KALMAN_FILTER_H 42 | #define IIROB_FILTERS_KALMAN_FILTER_H 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | 51 | namespace iirob_filters { 52 | template 53 | class MultiChannelKalmanFilter : public filters::MultiChannelFilterBase 54 | { 55 | public: 56 | MultiChannelKalmanFilter(); 57 | ~MultiChannelKalmanFilter(); 58 | virtual bool configure(); 59 | bool configure(const std::vector& init_state_vector, const std::string param_namespace = ""); 60 | bool configure(const std::string& param_namespace); 61 | virtual bool update(const std::vector& data_in, std::vector& data_out); 62 | bool update(const std::vector& data_in, std::vector& data_out, const double& delta_t, bool update_Q_matrix = false, const Eigen::MatrixXd& R_loc = Eigen::MatrixXd::Zero(1,1)); 63 | bool predict(std::vector& data_out); 64 | bool computePrediction(std::vector& data_out); 65 | bool computePrediction(std::vector& data_out, const double& delta_t); 66 | bool isInitializated(); 67 | bool getCurrentState(std::vector& data_out); 68 | bool getErrorCovarianceMatrix(Eigen::MatrixXd& data_out); 69 | bool resetErrorCovAndState(); 70 | bool getGatingMatrix(Eigen::MatrixXd& data_out); 71 | bool likelihood(const std::vector& data_in, double& data_out); 72 | 73 | private: 74 | // Matrices for computation 75 | Eigen::MatrixXd A, At, C, Q, Q_coeff, Q_exponent, R, P, K, P0, gatingMatrix; 76 | 77 | // System dimensions 78 | int m, n; 79 | 80 | // Discrete time step 81 | double dt; 82 | 83 | // Is the filter initialized? 84 | bool initialized; 85 | 86 | // Variance of process noise (for a time dependent Q) 87 | double Q_variance; 88 | 89 | // n-size identity 90 | Eigen::MatrixXd I; 91 | 92 | // Estimated states 93 | Eigen::VectorXd x_hat, x_hat_new; 94 | 95 | ros::NodeHandle nh; 96 | 97 | bool can_update_Q_matrix; 98 | 99 | bool fromStdVectorToEigenMatrix(std::vector& in, Eigen::MatrixXd& out, int rows, int columns, std::string matrix_name); 100 | bool fromStdVectorToEigenVector(std::vector& in, Eigen::VectorXd& out, int rows, std::string vector_name); 101 | 102 | double fac(double x); 103 | 104 | bool getParams(iirob_filters::KalmanFilterParameters&, const std::string&); 105 | 106 | bool isDynamicUpdate; 107 | }; 108 | 109 | template 110 | double MultiChannelKalmanFilter::fac(double x){ 111 | double f; 112 | if (x==0 || x==1) { 113 | f = 1; 114 | } 115 | else{ 116 | f = fac(x-1) * x; 117 | } 118 | return f; 119 | } 120 | 121 | template 122 | MultiChannelKalmanFilter::MultiChannelKalmanFilter() : nh("~") 123 | { 124 | initialized = isDynamicUpdate = can_update_Q_matrix = false; 125 | } 126 | 127 | template 128 | bool MultiChannelKalmanFilter::fromStdVectorToEigenMatrix(std::vector& in, Eigen::MatrixXd& out, int rows, 129 | int columns, std::string matrix_name) { 130 | if (in.size() != rows * columns || in.size() == 0) { ROS_ERROR("%s is not valid!", matrix_name.c_str()); return false; } 131 | out.resize(rows, columns); 132 | std::vector::iterator it = in.begin(); 133 | for (int i = 0; i < rows; ++i) 134 | { 135 | for (int j = 0; j < columns; ++j) 136 | { 137 | out(i, j) = *it; 138 | ++it; 139 | } 140 | } 141 | return true; 142 | } 143 | 144 | template 145 | bool MultiChannelKalmanFilter::isInitializated() 146 | { 147 | return initialized; 148 | } 149 | 150 | template 151 | bool MultiChannelKalmanFilter::fromStdVectorToEigenVector(std::vector& in, Eigen::VectorXd& out, int rows, std::string vector_name) { 152 | if (in.size() != rows) { ROS_ERROR("%s vector is not valid!", vector_name.c_str()); return false; } 153 | out.resize(rows); 154 | for (int i = 0; i < rows; ++i) 155 | { 156 | out(i) = in[i]; 157 | } 158 | return true; 159 | } 160 | 161 | template 162 | bool MultiChannelKalmanFilter::configure(const std::vector& init_state_vector, const std::string param_namespace) { 163 | 164 | if (!configure(param_namespace)) { return false; } 165 | 166 | if (init_state_vector.size() != n) { ROS_ERROR("Kalman: Not valid init state vector!"); return false; } 167 | 168 | for (int i = 0; i < init_state_vector.size(); i++) 169 | { 170 | x_hat(i) = init_state_vector[i]; 171 | } 172 | return true; 173 | } 174 | 175 | template 176 | bool MultiChannelKalmanFilter::configure() { 177 | return configure(""); 178 | } 179 | 180 | template 181 | bool MultiChannelKalmanFilter::configure(const std::string& param_namespace) { 182 | std::vector temp; 183 | std::string full_namespace = nh.getNamespace() + "/"; 184 | if (param_namespace != "") 185 | { 186 | full_namespace += param_namespace; 187 | iirob_filters::KalmanFilterParameters dynamic_params{full_namespace}; 188 | dynamic_params.fromParamServer(); 189 | if (!getParams(dynamic_params, full_namespace)) { return false; } 190 | } 191 | else 192 | { 193 | full_namespace += "KalmanFilter"; 194 | iirob_filters::KalmanFilterParameters default_namespace_params{full_namespace}; 195 | default_namespace_params.fromParamServer(); 196 | if (!getParams(default_namespace_params, full_namespace)) { return false; } 197 | } 198 | 199 | I = Eigen::MatrixXd::Zero(n, n); 200 | I.setIdentity(); 201 | 202 | x_hat_new = Eigen::VectorXd::Zero(n); 203 | P = P0; 204 | 205 | gatingMatrix = C * P * C.transpose() + R; 206 | 207 | initialized = true; 208 | return true; 209 | } 210 | 211 | template 212 | bool MultiChannelKalmanFilter::getParams(iirob_filters::KalmanFilterParameters& parameters, const std::string& param_namespace) 213 | { 214 | if(ros::param::has(std::string(param_namespace + "/" + "dt"))) 215 | { 216 | dt = parameters.dt; 217 | } 218 | else 219 | { 220 | ROS_ERROR("Kalman: dt is not available!"); 221 | return false; 222 | } 223 | 224 | if(ros::param::has(std::string(param_namespace + "/" + "n"))) 225 | { 226 | n = parameters.n; 227 | } 228 | else 229 | { 230 | ROS_ERROR("Kalman: n is not available!"); 231 | return false; 232 | } 233 | 234 | if(ros::param::has(std::string(param_namespace + "/" + "m"))) 235 | { 236 | m = parameters.m; 237 | } 238 | else 239 | { 240 | ROS_ERROR("Kalman: m is not available!"); 241 | return false; 242 | } 243 | 244 | if (dt <= 0 || n <= 0 || m <= 0) 245 | { 246 | ROS_ERROR("Kalman: dt, n or m is not valid! (dt <= 0 or n <= 0 or m <= 0)"); 247 | return false; 248 | } 249 | 250 | std::vector temp; 251 | 252 | if(ros::param::has(std::string(param_namespace + "/" + "A"))) 253 | { 254 | temp = parameters.A; 255 | if (!fromStdVectorToEigenMatrix(temp, A, n, n, "State matrix")) { return false; } 256 | } 257 | else 258 | { 259 | ROS_ERROR("A is not available!"); 260 | return false; 261 | } 262 | 263 | if(ros::param::has(std::string(param_namespace + "/" + "At"))) 264 | { 265 | temp = parameters.At; 266 | if (temp.size() == 0) 267 | { 268 | ROS_DEBUG("At is not used!"); 269 | } 270 | else if (fromStdVectorToEigenMatrix(temp, At, n, n, "Dynamic part of state matrix")) 271 | { 272 | isDynamicUpdate = true; 273 | } 274 | } 275 | else 276 | { 277 | ROS_DEBUG("At is not available!"); 278 | } 279 | 280 | if(ros::param::has(std::string(param_namespace + "/" + "C"))) 281 | { 282 | temp = parameters.C; 283 | if (!fromStdVectorToEigenMatrix(temp, C, m, n, "Output matrix")) { return false; } 284 | } 285 | else 286 | { 287 | ROS_ERROR("C is not available!"); 288 | return false; 289 | } 290 | 291 | if(ros::param::has(std::string(param_namespace + "/" + "Q"))) 292 | { 293 | temp = parameters.Q; 294 | if (!fromStdVectorToEigenMatrix(temp, Q, n, n, "Process noise covariance")) { return false; } 295 | } 296 | else 297 | { 298 | ROS_ERROR("Q is not available!"); 299 | return false; 300 | } 301 | 302 | bool skipDynamicPartQ = false; 303 | if(isDynamicUpdate && ros::param::has(std::string(param_namespace + "/" + "Q_coeff"))) 304 | { 305 | temp = parameters.Q_coeff; 306 | if (!fromStdVectorToEigenMatrix(temp, Q_coeff, n, n, 307 | "Process noise covariance (coefficients of dynamic part of Q)")) 308 | { 309 | skipDynamicPartQ = true; 310 | } 311 | } 312 | else 313 | { 314 | ROS_DEBUG("Q_coeff is not available!"); 315 | skipDynamicPartQ = true; 316 | } 317 | 318 | if(isDynamicUpdate && !skipDynamicPartQ && 319 | ros::param::has(std::string(param_namespace + "/" + "Q_exponent"))) 320 | { 321 | temp = parameters.Q_exponent; 322 | if (!fromStdVectorToEigenMatrix(temp, Q_exponent, n, n, 323 | "Process noise covariance (exponents of the time difference)")) 324 | { 325 | skipDynamicPartQ = true; 326 | } 327 | } 328 | else 329 | { 330 | ROS_DEBUG("Q_exponent is not available!"); 331 | skipDynamicPartQ = true; 332 | } 333 | 334 | if(isDynamicUpdate && !skipDynamicPartQ && 335 | ros::param::has(std::string(param_namespace + "/" + "Q_variance"))) 336 | { 337 | Q_variance = parameters.Q_variance; 338 | can_update_Q_matrix = true; 339 | } 340 | else 341 | { 342 | ROS_DEBUG("Q_variance is not available!"); 343 | } 344 | 345 | if(ros::param::has(std::string(param_namespace + "/" + "R"))) 346 | { 347 | temp = parameters.R; 348 | if (!fromStdVectorToEigenMatrix(temp, R, m, m, "Measurement noise covariance")) { return false; } 349 | } 350 | else 351 | { 352 | ROS_ERROR("R is not available!"); 353 | return false; 354 | } 355 | 356 | if(ros::param::has(std::string(param_namespace + "/" + "P"))) 357 | { 358 | temp = parameters.P; 359 | if (!fromStdVectorToEigenMatrix(temp, P0, n, n, "Estimate error covariance")) { return false; } 360 | } 361 | else 362 | { 363 | ROS_ERROR("P is not available!"); 364 | return false; 365 | } 366 | 367 | if(ros::param::has(std::string(param_namespace + "/" + "x0"))) 368 | { 369 | temp = parameters.x0; 370 | if (!fromStdVectorToEigenVector(temp, x_hat, n, "Start state vector")) { return false; } 371 | isDynamicUpdate = true; 372 | } 373 | else 374 | { 375 | ROS_DEBUG("x0 is not available!"); 376 | x_hat = Eigen::VectorXd::Zero(n); 377 | } 378 | 379 | return true; 380 | } 381 | 382 | template 383 | MultiChannelKalmanFilter::~MultiChannelKalmanFilter() 384 | {} 385 | 386 | template 387 | bool MultiChannelKalmanFilter::computePrediction(std::vector& data_out) 388 | { 389 | if(!initialized) { ROS_ERROR("Kalman: Filter is not initialized!"); return false; } 390 | 391 | Eigen::VectorXd v = Eigen::VectorXd::Zero(n); 392 | v = A * x_hat; 393 | 394 | data_out.resize(n); 395 | for (int i = 0; i < data_out.size(); ++i) { 396 | data_out[i] = v(i); 397 | } 398 | 399 | return true; 400 | } 401 | 402 | template 403 | bool MultiChannelKalmanFilter::computePrediction(std::vector& data_out, const double& delta_t) 404 | { 405 | if(!initialized) { ROS_ERROR("Kalman: Filter is not initialized!"); return false; } 406 | 407 | Eigen::MatrixXd A_t = At; 408 | 409 | for(int i = 0; i < n; i++){ 410 | for(int j = 0; j < n; j++){ 411 | if( A_t(i,j) ){ 412 | A_t(i,j) = pow( delta_t , A_t(i,j) )/fac( A_t(i,j) ); 413 | } 414 | } 415 | } 416 | 417 | Eigen::VectorXd v = Eigen::VectorXd::Zero(n); 418 | v = (A_t+A) * x_hat; 419 | 420 | data_out.resize(n); 421 | for (int i = 0; i < data_out.size(); ++i) { 422 | data_out[i] = v(i); 423 | } 424 | 425 | return true; 426 | } 427 | 428 | template 429 | bool MultiChannelKalmanFilter::predict(std::vector& data_out) 430 | { 431 | if(!initialized) { ROS_ERROR("Kalman: Filter is not initialized!"); return false; } 432 | 433 | x_hat = A * x_hat; 434 | P = A*P*A.transpose() + Q; 435 | 436 | data_out.resize(n); 437 | for (int i = 0; i < data_out.size(); ++i) { 438 | data_out[i] = x_hat(i); 439 | } 440 | 441 | return true; 442 | } 443 | 444 | template 445 | bool MultiChannelKalmanFilter::update(const std::vector& data_in, std::vector& data_out) 446 | { 447 | if(!initialized) { ROS_ERROR("Kalman: Filter is not initialized!"); return false; } 448 | if(data_in.size() != m) { ROS_ERROR("Kalman: Not valid measurement vector!"); return false; } 449 | 450 | Eigen::VectorXd y(m); 451 | for (int i = 0; i < m; ++i) { 452 | y(i) = data_in[i]; 453 | } 454 | 455 | x_hat_new = A * x_hat; 456 | P = A*P*A.transpose() + Q; 457 | gatingMatrix = C * P * C.transpose() + R; 458 | K = P*C.transpose()*gatingMatrix.inverse(); // nxm 459 | x_hat_new += K * (y - C*x_hat_new); 460 | P = (I - K*C)*P; 461 | x_hat = x_hat_new; 462 | 463 | data_out.resize(n); 464 | for (int i = 0; i < data_out.size(); ++i) { 465 | data_out[i] = x_hat(i); 466 | } 467 | 468 | return true; 469 | } 470 | 471 | template 472 | bool MultiChannelKalmanFilter::getGatingMatrix(Eigen::MatrixXd& data_out) 473 | { 474 | if(!initialized) { ROS_ERROR("Kalman: Filter is not initialized!"); return false; } 475 | data_out = gatingMatrix; 476 | return true; 477 | } 478 | 479 | template 480 | bool MultiChannelKalmanFilter::getErrorCovarianceMatrix(Eigen::MatrixXd& data_out) 481 | { 482 | if(!initialized) { ROS_ERROR("Kalman: Filter is not initialized!"); return false; } 483 | data_out = P; 484 | return true; 485 | } 486 | 487 | template 488 | bool MultiChannelKalmanFilter::update(const std::vector& data_in, std::vector& data_out, 489 | const double& delta_t, bool update_Q_matrix, const Eigen::MatrixXd& R_loc) 490 | { 491 | if(!initialized) { ROS_ERROR("Kalman: Filter is not initialized!"); return false; } 492 | if(!isDynamicUpdate) { ROS_ERROR("Kalman: Dynamic update is not available!"); return false; } 493 | if(data_in.size() != m) { ROS_ERROR("Kalman: Not valid measurement vector!"); return false; } 494 | 495 | Eigen::VectorXd y(m); 496 | for (int i = 0; i < m; ++i) { 497 | y(i) = data_in[i]; 498 | } 499 | 500 | Eigen::MatrixXd A_t = At; 501 | 502 | for(int i = 0; i < n; i++){ 503 | for(int j = 0; j < n; j++){ 504 | if( A_t(i,j) ){ 505 | A_t(i,j) = pow( delta_t , A_t(i,j) )/fac( A_t(i,j) ); 506 | } 507 | } 508 | } 509 | 510 | //update Q matrix if requested 511 | Eigen::MatrixXd Q_updated = Q; 512 | if (can_update_Q_matrix && update_Q_matrix) 513 | { 514 | for(int i = 0; i < n; i++){ 515 | for(int j = 0; j < n; j++){ 516 | // adds x*delta_t^y to entry in Q 517 | Q_updated(i,j) = Q(i,j) + Q_variance * Q_coeff(i,j)*pow(delta_t, Q_exponent(i,j)); 518 | } 519 | } 520 | } 521 | 522 | Eigen::MatrixXd R_used = R; 523 | if(!R_loc.isZero() && R_loc.cols() == R.cols() && R_loc.rows() == R.rows()) 524 | R_used = R_loc; 525 | 526 | 527 | x_hat_new = (A_t+A) * x_hat; 528 | P = (A_t+A)*P*((A_t+A).transpose()) + Q_updated; 529 | gatingMatrix = C * P * C.transpose() + R_used; 530 | K = P*C.transpose()*gatingMatrix.inverse(); // nxm 531 | x_hat_new += K * (y - C*x_hat_new); 532 | P = (I - K*C)*P; 533 | x_hat = x_hat_new; 534 | 535 | data_out.resize(n); 536 | for (int i = 0; i < n; ++i) { 537 | data_out[i] = x_hat(i); 538 | } 539 | return true; 540 | } 541 | 542 | template 543 | bool MultiChannelKalmanFilter::getCurrentState(std::vector& data_out) 544 | { 545 | if(!initialized) { ROS_ERROR("Kalman: Filter is not initialized!"); return false; } 546 | data_out.resize(n); 547 | for (int i = 0; i < n; ++i) { 548 | data_out[i] = x_hat(i); 549 | } 550 | return true; 551 | } 552 | 553 | template 554 | bool MultiChannelKalmanFilter::likelihood(const std::vector& data_in, double& data_out) 555 | { 556 | if(!initialized) { ROS_ERROR("Kalman: Filter is not initialized!"); return false; } 557 | 558 | if(data_in.size() != m) { ROS_ERROR("Kalman: Not valid measurement vector!"); return false; } 559 | 560 | Eigen::VectorXd measurement(m); 561 | for (int i = 0; i < m; ++i) { 562 | measurement(i) = data_in[i]; 563 | } 564 | 565 | // convert prediction to measurement space 566 | Eigen::VectorXd prediction = C * A * x_hat; 567 | 568 | // vector of prediction (origin = current state) 569 | Eigen::VectorXd continuousPrediction = prediction - C * x_hat; 570 | 571 | // assumed interpolated prediction = current state + dt * prediction 572 | Eigen::VectorXd timeShiftedPrediction = ( C * x_hat ) + ( dt * continuousPrediction ); 573 | 574 | Eigen::VectorXd d = timeShiftedPrediction - measurement; 575 | 576 | // calculate exponent 577 | const double e = -0.5 * d.transpose() * gatingMatrix.inverse() * d; 578 | 579 | // get normal distribution value 580 | data_out = (1. / (std::pow(2. * M_PI, (double) m * .5) 581 | * std::sqrt(gatingMatrix.determinant()))) * std::exp(e); 582 | 583 | return true; 584 | } 585 | 586 | template 587 | bool MultiChannelKalmanFilter::resetErrorCovAndState() 588 | { 589 | if(!initialized) { ROS_ERROR("Kalman: Filter is not initialized!"); return false; } 590 | 591 | for (int i = m; i < n; i++) 592 | { 593 | x_hat(i) = 0.0; 594 | } 595 | P = P0; 596 | return true; 597 | } 598 | 599 | 600 | } 601 | #endif 602 | -------------------------------------------------------------------------------- /include/iirob_filters/low_pass_filter.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * 3 | * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group, 4 | * Institute for Anthropomatics and Robotics (IAR) - 5 | * Intelligent Process Control and Robotics (IPR), 6 | * Karlsruhe Institute of Technology (KIT) 7 | * 8 | * Author: Denis Štogl, email: denis.stogl@kit.edu 9 | * 10 | * Date of creation: 2016 11 | * 12 | * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 13 | * 14 | * Redistribution and use in source and binary forms, 15 | * with or without modification, are permitted provided 16 | * that the following conditions are met: 17 | * 18 | * * Redistributions of source code must retain the above copyright 19 | * notice, this list of conditions and the following disclaimer. 20 | * * Redistributions in binary form must reproduce the above copyright 21 | * notice, this list of conditions and the following disclaimer in the 22 | * documentation and/or other materials provided with the distribution. 23 | * * Neither the name of the copyright holder nor the names of its 24 | * contributors may be used to endorse or promote products derived from 25 | * this software without specific prior written permission. 26 | * 27 | * This package is free software: you can redistribute it and/or modify 28 | * it under the terms of the GNU Lesser General Public License as published by 29 | * the Free Software Foundation, either version 3 of the License, or 30 | * (at your option) any later version. 31 | * 32 | * This package is distributed in the hope that it will be useful, 33 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 34 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 35 | * GNU Lesser General Public License for more details. 36 | * 37 | * You should have received a copy of the GNU Lesser General Public License 38 | * along with this package. If not, see . 39 | *****************************************************************************/ 40 | 41 | #ifndef IIROB_FILTERS_LOW_PASS_FILTER_H 42 | #define IIROB_FILTERS_LOW_PASS_FILTER_H 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include 53 | 54 | namespace iirob_filters{ 55 | template 56 | class LowPassFilter: public filters::FilterBase 57 | { 58 | public: 59 | LowPassFilter(); 60 | 61 | ~LowPassFilter(); 62 | virtual bool configure(); 63 | virtual bool update(const T& data_in, T& data_out); 64 | 65 | private: 66 | // Parameters 67 | double sampling_frequency_; 68 | double damping_frequency_; 69 | double damping_intensity_; 70 | int divider_; 71 | std::map map_param_; 72 | 73 | // Filter parameters 74 | double b1; 75 | double a1; 76 | int divider_counter; 77 | iirob_filters::LowPassFilterParameters params_; 78 | double filtered_value, filtered_old_value, old_value; 79 | 80 | Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; 81 | 82 | dynamic_reconfigure::Server reconfigCalibrationSrv_; // Dynamic reconfiguration service 83 | void reconfigureConfigurationRequest(iirob_filters::LowPassFilterConfig& config, uint32_t level); 84 | }; 85 | 86 | 87 | template 88 | LowPassFilter::LowPassFilter():params_{ros::NodeHandle("~/LowPassFilter/params").getNamespace()} 89 | { 90 | reconfigCalibrationSrv_.setCallback(boost::bind(&LowPassFilter::reconfigureConfigurationRequest, this, _1, _2)); 91 | } 92 | 93 | template 94 | LowPassFilter::~LowPassFilter() 95 | { 96 | } 97 | 98 | template 99 | bool LowPassFilter::configure() 100 | { 101 | params_.fromParamServer(); 102 | sampling_frequency_ = params_.SamplingFrequency; 103 | damping_frequency_ = params_.DampingFrequency; 104 | damping_intensity_ = params_.DampingIntensity; 105 | divider_ = params_.divider; 106 | if(sampling_frequency_ == 0) 107 | ROS_ERROR("LowPassFilter did not find param SamplingFrequency"); 108 | if(damping_frequency_ == 0) 109 | ROS_ERROR("LowPassFilter did not find param DampingFrequency"); 110 | if(damping_intensity_ == 0) 111 | ROS_ERROR("LowPassFilter did not find param DampingIntensity"); 112 | if(divider_ == 0) 113 | ROS_ERROR("Divider value not correct - cannot be 0. Check .param or .yaml files"); 114 | 115 | ROS_INFO("Low Pass Filter Params: Sampling Frequency:%f, Damping Frequency:%f, Damping Intensity:%f; Divider: %d " , 116 | sampling_frequency_, damping_frequency_, damping_intensity_, divider_); 117 | 118 | a1 = exp(-1 / sampling_frequency_ * (2 * M_PI * damping_frequency_) / (pow(10, damping_intensity_ / -10.0))); 119 | b1 = 1 - a1; 120 | 121 | divider_counter = 1; 122 | // Initialize storage Vectors 123 | filtered_value = filtered_old_value = old_value = 0; 124 | for (int ii=0; ii<6; ii++) 125 | { 126 | msg_filtered(ii) = msg_filtered_old(ii) = msg_old(ii) = 0; 127 | } 128 | return true; 129 | } 130 | 131 | template<> 132 | inline bool LowPassFilter::update(const geometry_msgs::WrenchStamped& data_in, geometry_msgs::WrenchStamped& data_out){ 133 | // IIR Filter 134 | msg_filtered = b1 * msg_old + a1 * msg_filtered_old; 135 | msg_filtered_old = msg_filtered; 136 | 137 | //TODO use wrenchMsgToEigen 138 | msg_old[0] = data_in.wrench.force.x; 139 | msg_old[1] = data_in.wrench.force.y; 140 | msg_old[2] = data_in.wrench.force.z; 141 | msg_old[3] = data_in.wrench.torque.x; 142 | msg_old[4] = data_in.wrench.torque.y; 143 | msg_old[5] = data_in.wrench.torque.z; 144 | 145 | data_out.wrench.force.x = msg_filtered[0]; 146 | data_out.wrench.force.y = msg_filtered[1]; 147 | data_out.wrench.force.z = msg_filtered[2]; 148 | data_out.wrench.torque.x = msg_filtered[3]; 149 | data_out.wrench.torque.y = msg_filtered[4]; 150 | data_out.wrench.torque.z = msg_filtered[5]; 151 | return true; 152 | } 153 | 154 | template 155 | bool LowPassFilter::update(const T& data_in, T& data_out) 156 | { 157 | data_out = b1 * old_value + a1 * filtered_old_value; 158 | filtered_old_value = data_out; 159 | old_value = data_in; 160 | 161 | return true; 162 | } 163 | 164 | template 165 | void LowPassFilter::reconfigureConfigurationRequest(iirob_filters::LowPassFilterConfig& config, uint32_t level) 166 | { 167 | //params_.fromConfig(config); 168 | sampling_frequency_ = params_.SamplingFrequency; 169 | damping_intensity_ = params_.DampingFrequency; 170 | damping_intensity_ = params_.DampingIntensity; 171 | divider_ = params_.divider; 172 | }; 173 | 174 | /* A lp filter which works on double arrays. 175 | * 176 | */ 177 | template 178 | class MultiChannelLowPassFilter: public filters::MultiChannelFilterBase 179 | { 180 | public: 181 | /** \brief Construct the filter with the expected width and height */ 182 | MultiChannelLowPassFilter(); 183 | 184 | /** \brief Destructor to clean up 185 | */ 186 | ~MultiChannelLowPassFilter(); 187 | 188 | virtual bool configure(); 189 | 190 | /** \brief Update the filter and return the data seperately 191 | * \param data_in T array with length width 192 | * \param data_out T array with length width 193 | */ 194 | virtual bool update( const std::vector & data_in, std::vector& data_out); 195 | 196 | protected: 197 | 198 | double sampling_frequency_; 199 | double damping_frequency_; 200 | double damping_intensity_; 201 | int divider_; 202 | 203 | // Filter parametrs 204 | int divider_counter; 205 | double b1; 206 | double a1; 207 | 208 | iirob_filters::LowPassFilterParameters params_; 209 | std::vector filtered_value, filtered_old_value, old_value; 210 | 211 | using filters::MultiChannelFilterBase::number_of_channels_; 212 | }; 213 | 214 | 215 | template 216 | MultiChannelLowPassFilter::MultiChannelLowPassFilter():params_{ros::NodeHandle("~/LowPassFilter/params").getNamespace()} 217 | { 218 | } 219 | 220 | template 221 | MultiChannelLowPassFilter::~MultiChannelLowPassFilter() 222 | { 223 | } 224 | 225 | template 226 | bool MultiChannelLowPassFilter::configure() 227 | { 228 | params_.fromParamServer(); 229 | sampling_frequency_ = params_.SamplingFrequency; 230 | damping_frequency_ = params_.DampingFrequency; 231 | damping_intensity_ = params_.DampingIntensity; 232 | divider_ = params_.divider; 233 | if(sampling_frequency_ == 0) 234 | ROS_ERROR("MultiChannelLowPassFilter did not find param SamplingFrequency"); 235 | if(damping_frequency_ == 0) 236 | ROS_ERROR("MultiChannelLowPassFilter did not find param DampingFrequency"); 237 | if(damping_intensity_ == 0) 238 | ROS_ERROR("MultiChannelLowPassFilter did not find param DampingIntensity"); 239 | if(divider_ == 0) 240 | ROS_ERROR("Divider value not correct - cannot be 0. Check .param or .yaml files"); 241 | 242 | ROS_INFO("Low Pass Filter Params: Sampling Frequency:%f, Damping Frequency:%f, Damping Intensity:%f; Divider: %d " , 243 | sampling_frequency_, damping_frequency_, damping_intensity_, divider_); 244 | 245 | a1 = exp(-1 / sampling_frequency_ * (2 * M_PI * damping_frequency_) / (pow(10, damping_intensity_ / -10.0))); 246 | b1 = 1 - a1; 247 | divider_counter = 1; 248 | 249 | // Initialize storage Vectors 250 | 251 | for (int ii=0; ii 259 | bool MultiChannelLowPassFilter::update(const std::vector & data_in, std::vector& data_out) 260 | { 261 | 262 | if (data_in.size() != number_of_channels_ || data_out.size() != number_of_channels_) 263 | { 264 | ROS_ERROR("Configured with wrong size config:%d in:%d out:%d", number_of_channels_, (int)data_in.size(), (int)data_out.size()); 265 | return false; 266 | } 267 | for (int i = 0; i. 39 | *****************************************************************************/ 40 | 41 | #ifndef IIROB_FILTERS_MOVING_MEAN_FILTER_H 42 | #define IIROB_FILTERS_MOVING_MEAN_FILTER_H 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | namespace iirob_filters{ 50 | 51 | template 52 | class MovingMeanFilter: public filters::FilterBase 53 | { 54 | public: 55 | 56 | MovingMeanFilter(); 57 | ~MovingMeanFilter(); 58 | 59 | virtual bool configure(); 60 | virtual bool update(const T & data_in, T& data_out); 61 | 62 | private: 63 | // Parameters 64 | iirob_filters::MovingMeanParameters params_; 65 | int divider_; 66 | 67 | // Filter parametrs 68 | int divider_counter; 69 | std::vector values; 70 | }; 71 | 72 | template 73 | MovingMeanFilter::MovingMeanFilter():params_{ros::NodeHandle("~/MovingMeanFilter/params").getNamespace()} 74 | { 75 | } 76 | 77 | template 78 | MovingMeanFilter::~MovingMeanFilter() 79 | { 80 | } 81 | 82 | template 83 | bool MovingMeanFilter::configure() 84 | { 85 | params_.fromParamServer(); 86 | divider_ = params_.divider; 87 | 88 | ROS_INFO("Moving Mean Filter Params: Divider: %d " , divider_); 89 | 90 | if(divider_ == 0) 91 | ROS_ERROR("MovingMeanFilter did not find param divider"); 92 | return true; 93 | } 94 | 95 | template 96 | bool MovingMeanFilter::update(const T & data_in, T& data_out) 97 | { 98 | if (values.size() < divider_) { 99 | values.push_back(data_in); 100 | data_out = data_in; 101 | return true; 102 | } 103 | 104 | values.erase(values.begin()); 105 | values.push_back(data_in); 106 | 107 | T sum; 108 | sum.wrench.force.x = 0.0; 109 | sum.wrench.force.y = 0.0; 110 | sum.wrench.force.z = 0.0; 111 | sum.wrench.torque.x = 0.0; 112 | sum.wrench.torque.y = 0.0; 113 | sum.wrench.torque.x = 0.0; 114 | for(int i =0 ; i. 40 | *****************************************************************************/ 41 | 42 | #ifndef IIROB_FILTERS_THRESHOLD_FILTER_H 43 | #define IIROB_FILTERS_THRESHOLD_FILTER_H 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | namespace iirob_filters{ 53 | template 54 | class ThresholdFilter: public filters::FilterBase 55 | { 56 | public: 57 | ThresholdFilter(); 58 | 59 | ~ThresholdFilter(); 60 | virtual bool configure(); 61 | virtual bool update(const T & data_in, T& data_out); 62 | 63 | private: 64 | iirob_filters::ThresholdParameters params_; 65 | double threshold_; 66 | double threshold_lin_; 67 | double threshold_angular_; 68 | 69 | dynamic_reconfigure::Server reconfigCalibrationSrv_; // Dynamic reconfiguration service 70 | 71 | void reconfigureConfigurationRequest(iirob_filters::ThresholdConfig& config, uint32_t level); 72 | 73 | }; 74 | 75 | template 76 | ThresholdFilter::ThresholdFilter(): params_{ros::NodeHandle("~/ThresholdFilter/params").getNamespace()} 77 | { 78 | reconfigCalibrationSrv_.setCallback(boost::bind(&ThresholdFilter::reconfigureConfigurationRequest, this, _1, _2)); 79 | } 80 | 81 | template 82 | ThresholdFilter::~ThresholdFilter() 83 | { 84 | } 85 | 86 | template 87 | bool ThresholdFilter::configure() 88 | { 89 | params_.fromParamServer(); 90 | threshold_ = params_.threshold; 91 | threshold_lin_ = params_.linear_threshold; 92 | threshold_angular_ = params_.angular_threshold; 93 | if(threshold_lin_ == 0) 94 | ROS_ERROR("ThresholdFilter did not find param linear_threshold"); 95 | if(threshold_angular_ == 0) 96 | ROS_ERROR("ThresholdFilter did not find param angular_threshold"); 97 | 98 | ROS_INFO("Threshhold Filter Params: Threshold: %f; Treshold lin.: %f; Threshold Anglular: %f" , 99 | threshold_, threshold_lin_, threshold_angular_); 100 | 101 | return true; 102 | } 103 | 104 | template 105 | bool ThresholdFilter::update(const T & data_in, T& data_out) 106 | { 107 | data_out = data_in; 108 | 109 | if (fabs(data_in) > threshold_) { 110 | double sign = (data_in > 0) ? 1 : -1; 111 | data_out = threshold_*sign; 112 | } 113 | return true; 114 | } 115 | 116 | template <> 117 | inline bool ThresholdFilter::update(const geometry_msgs::WrenchStamped & data_in, geometry_msgs::WrenchStamped& data_out) 118 | { 119 | data_out = data_in; 120 | 121 | if (fabs(data_in.wrench.force.x) > threshold_lin_) 122 | { 123 | double sign = (data_in.wrench.force.x > 0) ? 1 : -1; 124 | data_out.wrench.force.x = data_in.wrench.force.x-threshold_lin_*sign; 125 | } 126 | else 127 | { 128 | data_out.wrench.force.x = 0; 129 | } 130 | if (fabs(data_in.wrench.force.y) > threshold_lin_) 131 | { 132 | double sign = (data_in.wrench.force.y > 0) ? 1 : -1; 133 | data_out.wrench.force.y = data_in.wrench.force.y-threshold_lin_*sign; 134 | } 135 | else 136 | { 137 | data_out.wrench.force.y = 0; 138 | } 139 | if (fabs(data_in.wrench.force.z) > threshold_lin_) 140 | { 141 | double sign = (data_in.wrench.force.z > 0) ? 1 : -1; 142 | data_out.wrench.force.z = data_in.wrench.force.z-threshold_lin_*sign; 143 | } 144 | else 145 | { 146 | data_out.wrench.force.z = 0; 147 | } 148 | if (fabs(data_in.wrench.torque.x) > threshold_angular_) 149 | { 150 | double sign = (data_in.wrench.torque.x > 0) ? 1 : -1; 151 | data_out.wrench.torque.x = data_in.wrench.torque.x-threshold_angular_*sign; 152 | } 153 | else 154 | { 155 | data_out.wrench.torque.x = 0; 156 | } 157 | if (fabs(data_in.wrench.torque.y) > threshold_angular_) 158 | { 159 | double sign = (data_in.wrench.force.y > 0) ? 1 : -1; 160 | data_out.wrench.torque.y = data_in.wrench.torque.y-threshold_angular_*sign; 161 | } 162 | else 163 | { 164 | data_out.wrench.torque.y = 0; 165 | } 166 | if (fabs(data_in.wrench.torque.z) > threshold_angular_) 167 | { 168 | double sign = (data_in.wrench.torque.z > 0) ? 1 : -1; 169 | data_out.wrench.torque.z = data_in.wrench.torque.z-threshold_angular_*sign; 170 | } 171 | else 172 | { 173 | data_out.wrench.torque.z = 0; 174 | } 175 | return true; 176 | } 177 | 178 | template 179 | void ThresholdFilter::reconfigureConfigurationRequest(iirob_filters::ThresholdConfig& config, uint32_t level) 180 | { 181 | //params_.fromConfig(config); 182 | threshold_ = params_.threshold; 183 | threshold_lin_ = params_.linear_threshold; 184 | threshold_angular_ = params_.angular_threshold; 185 | }; 186 | 187 | template 188 | class MultiChannelThresholdFilter: public filters::MultiChannelFilterBase 189 | { 190 | public: 191 | MultiChannelThresholdFilter(); 192 | ~MultiChannelThresholdFilter(); 193 | 194 | virtual bool configure(); 195 | virtual bool update(const std::vector& data_in, std::vector& data_out); 196 | 197 | private: 198 | 199 | //ROS Objects 200 | iirob_filters::ThresholdParameters params_; 201 | double threshold_; 202 | double threshold_lin_; 203 | double threshold_angular_; 204 | 205 | using filters::MultiChannelFilterBase::number_of_channels_; 206 | }; 207 | 208 | template 209 | MultiChannelThresholdFilter::MultiChannelThresholdFilter(): params_{ros::NodeHandle("~/ThresholdFilter/params").getNamespace()} 210 | { 211 | } 212 | 213 | template 214 | MultiChannelThresholdFilter::~MultiChannelThresholdFilter() 215 | { 216 | } 217 | 218 | 219 | template 220 | bool MultiChannelThresholdFilter::configure() 221 | { 222 | params_.fromParamServer(); 223 | threshold_ = params_.threshold; 224 | threshold_lin_ = params_.linear_threshold; 225 | threshold_angular_ = params_.angular_threshold; 226 | if(threshold_lin_ == 0) 227 | ROS_ERROR("ThresholdFilter did not find param linear_threshold"); 228 | if(threshold_angular_ == 0) 229 | ROS_ERROR("ThresholdFilter did not find param angular_threshold"); 230 | 231 | return true; 232 | } 233 | 234 | template 235 | bool MultiChannelThresholdFilter::update(const std::vector& data_in, std::vector& data_out) 236 | { 237 | 238 | 239 | if (data_in.size() != number_of_channels_ || data_out.size() != number_of_channels_) 240 | { 241 | ROS_ERROR("Configured with wrong size config:%d in:%d out:%d", number_of_channels_, (int)data_in.size(), (int)data_out.size()); 242 | return false; 243 | } 244 | 245 | for(int i = 0; i < data_in.size(); i++) 246 | { 247 | data_out[i] = data_in[i]; 248 | if (fabs(data_in[i]) > threshold_) { 249 | double sign = (data_in[i] > 0) ? 1 : -1; 250 | data_out[i] = threshold_*sign; 251 | } 252 | else 253 | { 254 | data_out[i] = 0; 255 | } 256 | } 257 | return true; 258 | }; 259 | } 260 | #endif 261 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | iirob_filters 4 | 0.9.2 5 | The iirob_filters package implements following filters: 6 | 1) Low-Pass 7 | 2) Moving Mean 8 | 3) Gravity Compensation (used for force-torque sensors) 9 | 4) Threshold Filter 10 | 5) Kalman Filter 11 | 12 | 13 | Denis Štogl 14 | 15 | Apache2.0 16 | 17 | http://wiki.ros.org/iirob_filters 18 | https://github.com/KITrobotics/iirob_filters 19 | https://github.com/KITrobotics/iirob_filtersissues 20 | 21 | Denis Štogl 22 | 23 | catkin 24 | 25 | cmake_modules 26 | dynamic_reconfigure 27 | eigen_conversions 28 | filters 29 | geometry_msgs 30 | pluginlib 31 | roscpp 32 | rosparam_handler 33 | tf2_geometry_msgs 34 | tf2_ros 35 | rostest 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /src/gravity_compensation.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * 3 | * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group, 4 | * Institute for Anthropomatics and Robotics (IAR) - 5 | * Intelligent Process Control and Robotics (IPR), 6 | * Karlsruhe Institute of Technology (KIT) 7 | * 8 | * Author: Alexandar Pollmann 9 | * Denis Štogl, email: denis.stogl@kit.edu 10 | * 11 | * Date of creation: 2015-2016 12 | * 13 | * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 14 | * 15 | * Redistribution and use in source and binary forms, 16 | * with or without modification, are permitted provided 17 | * that the following conditions are met: 18 | * 19 | * * Redistributions of source code must retain the above copyright 20 | * notice, this list of conditions and the following disclaimer. 21 | * * Redistributions in binary form must reproduce the above copyright 22 | * notice, this list of conditions and the following disclaimer in the 23 | * documentation and/or other materials provided with the distribution. 24 | * * Neither the name of the copyright holder nor the names of its 25 | * contributors may be used to endorse or promote products derived from 26 | * this software without specific prior written permission. 27 | * 28 | * This package is free software: you can redistribute it and/or modify 29 | * it under the terms of the GNU Lesser General Public License as published by 30 | * the Free Software Foundation, either version 3 of the License, or 31 | * (at your option) any later version. 32 | * 33 | * This package is distributed in the hope that it will be useful, 34 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 35 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 36 | * GNU Lesser General Public License for more details. 37 | * 38 | * You should have received a copy of the GNU Lesser General Public License 39 | * along with this package. If not, see . 40 | *****************************************************************************/ 41 | #include 42 | #include 43 | 44 | PLUGINLIB_EXPORT_CLASS(iirob_filters::GravityCompensator, filters::FilterBase) 45 | 46 | -------------------------------------------------------------------------------- /src/kalman_filter.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * 3 | * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group, 4 | * Institute for Anthropomatics and Robotics (IAR) - 5 | * Intelligent Process Control and Robotics (IPR), 6 | * Karlsruhe Institute of Technology (KIT) 7 | * 8 | * Author: Denis Štogl, email: denis.stogl@kit.edu 9 | * 10 | * Date of creation: 2017 11 | * 12 | * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 13 | * 14 | * Redistribution and use in source and binary forms, 15 | * with or without modification, are permitted provided 16 | * that the following conditions are met: 17 | * 18 | * * Redistributions of source code must retain the above copyright 19 | * notice, this list of conditions and the following disclaimer. 20 | * * Redistributions in binary form must reproduce the above copyright 21 | * notice, this list of conditions and the following disclaimer in the 22 | * documentation and/or other materials provided with the distribution. 23 | * * Neither the name of the copyright holder nor the names of its 24 | * contributors may be used to endorse or promote products derived from 25 | * this software without specific prior written permission. 26 | * 27 | * This package is free software: you can redistribute it and/or modify 28 | * it under the terms of the GNU Lesser General Public License as published by 29 | * the Free Software Foundation, either version 3 of the License, or 30 | * (at your option) any later version. 31 | * 32 | * This package is distributed in the hope that it will be useful, 33 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 34 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 35 | * GNU Lesser General Public License for more details. 36 | * 37 | * You should have received a copy of the GNU Lesser General Public License 38 | * along with this package. If not, see . 39 | *****************************************************************************/ 40 | 41 | #include 42 | #include 43 | 44 | PLUGINLIB_EXPORT_CLASS(iirob_filters::MultiChannelKalmanFilter, filters::MultiChannelFilterBase) 45 | -------------------------------------------------------------------------------- /src/low_pass_filter.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * 3 | * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group, 4 | * Institute for Anthropomatics and Robotics (IAR) - 5 | * Intelligent Process Control and Robotics (IPR), 6 | * Karlsruhe Institute of Technology (KIT) 7 | * 8 | * Author: Denis Štogl, email: denis.stogl@kit.edu 9 | * 10 | * Date of creation: 2016 11 | * 12 | * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 13 | * 14 | * Redistribution and use in source and binary forms, 15 | * with or without modification, are permitted provided 16 | * that the following conditions are met: 17 | * 18 | * * Redistributions of source code must retain the above copyright 19 | * notice, this list of conditions and the following disclaimer. 20 | * * Redistributions in binary form must reproduce the above copyright 21 | * notice, this list of conditions and the following disclaimer in the 22 | * documentation and/or other materials provided with the distribution. 23 | * * Neither the name of the copyright holder nor the names of its 24 | * contributors may be used to endorse or promote products derived from 25 | * this software without specific prior written permission. 26 | * 27 | * This package is free software: you can redistribute it and/or modify 28 | * it under the terms of the GNU Lesser General Public License as published by 29 | * the Free Software Foundation, either version 3 of the License, or 30 | * (at your option) any later version. 31 | * 32 | * This package is distributed in the hope that it will be useful, 33 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 34 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 35 | * GNU Lesser General Public License for more details. 36 | * 37 | * You should have received a copy of the GNU Lesser General Public License 38 | * along with this package. If not, see . 39 | *****************************************************************************/ 40 | #include 41 | #include 42 | 43 | PLUGINLIB_EXPORT_CLASS(iirob_filters::LowPassFilter, filters::FilterBase) 44 | PLUGINLIB_EXPORT_CLASS(iirob_filters::LowPassFilter, filters::FilterBase) 45 | PLUGINLIB_EXPORT_CLASS(iirob_filters::MultiChannelLowPassFilter, filters::MultiChannelFilterBase) 46 | -------------------------------------------------------------------------------- /src/moving_mean_filter.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * 3 | * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group, 4 | * Institute for Anthropomatics and Robotics (IAR) - 5 | * Intelligent Process Control and Robotics (IPR), 6 | * Karlsruhe Institute of Technology (KIT) 7 | * 8 | * Author: Alexandar Pollmann 9 | * Denis Štogl, email: denis.stogl@kit.edu 10 | * 11 | * Date of creation: 2015-2016 12 | * 13 | * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 14 | * 15 | * Redistribution and use in source and binary forms, 16 | * with or without modification, are permitted provided 17 | * that the following conditions are met: 18 | * 19 | * * Redistributions of source code must retain the above copyright 20 | * notice, this list of conditions and the following disclaimer. 21 | * * Redistributions in binary form must reproduce the above copyright 22 | * notice, this list of conditions and the following disclaimer in the 23 | * documentation and/or other materials provided with the distribution. 24 | * * Neither the name of the copyright holder nor the names of its 25 | * contributors may be used to endorse or promote products derived from 26 | * this software without specific prior written permission. 27 | * 28 | * This package is free software: you can redistribute it and/or modify 29 | * it under the terms of the GNU Lesser General Public License as published by 30 | * the Free Software Foundation, either version 3 of the License, or 31 | * (at your option) any later version. 32 | * 33 | * This package is distributed in the hope that it will be useful, 34 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 35 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 36 | * GNU Lesser General Public License for more details. 37 | * 38 | * You should have received a copy of the GNU Lesser General Public License 39 | * along with this package. If not, see . 40 | *****************************************************************************/ 41 | #include 42 | #include 43 | 44 | PLUGINLIB_EXPORT_CLASS(iirob_filters::MovingMeanFilter, filters::FilterBase) 45 | -------------------------------------------------------------------------------- /src/threshold_filter.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * 3 | * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group, 4 | * Institute for Anthropomatics and Robotics (IAR) - 5 | * Intelligent Process Control and Robotics (IPR), 6 | * Karlsruhe Institute of Technology (KIT) 7 | * 8 | * Author: Alexandar Pollmann 9 | * Denis Štogl, email: denis.stogl@kit.edu 10 | * 11 | * Date of creation: 2015-2016 12 | * 13 | * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 14 | * 15 | * Redistribution and use in source and binary forms, 16 | * with or without modification, are permitted provided 17 | * that the following conditions are met: 18 | * 19 | * * Redistributions of source code must retain the above copyright 20 | * notice, this list of conditions and the following disclaimer. 21 | * * Redistributions in binary form must reproduce the above copyright 22 | * notice, this list of conditions and the following disclaimer in the 23 | * documentation and/or other materials provided with the distribution. 24 | * * Neither the name of the copyright holder nor the names of its 25 | * contributors may be used to endorse or promote products derived from 26 | * this software without specific prior written permission. 27 | * 28 | * This package is free software: you can redistribute it and/or modify 29 | * it under the terms of the GNU Lesser General Public License as published by 30 | * the Free Software Foundation, either version 3 of the License, or 31 | * (at your option) any later version. 32 | * 33 | * This package is distributed in the hope that it will be useful, 34 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 35 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 36 | * GNU Lesser General Public License for more details. 37 | * 38 | * You should have received a copy of the GNU Lesser General Public License 39 | * along with this package. If not, see . 40 | *****************************************************************************/ 41 | #include 42 | #include 43 | 44 | PLUGINLIB_EXPORT_CLASS(iirob_filters::ThresholdFilter, filters::FilterBase) 45 | PLUGINLIB_EXPORT_CLASS(iirob_filters::ThresholdFilter, filters::FilterBase) 46 | PLUGINLIB_EXPORT_CLASS(iirob_filters::MultiChannelThresholdFilter, filters::MultiChannelFilterBase) 47 | -------------------------------------------------------------------------------- /test/how_to_run_tests.md: -------------------------------------------------------------------------------- 1 | 2 | "rostest iirob_filters test_kalman.launch" runs tests with output in xml 3 | 4 | "rostest iirob_filters test_kalman.launch --text" runs tests with output in console 5 | 6 | -------------------------------------------------------------------------------- /test/test_kalman.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | // Declare a test 9 | // TEST(TestSuite, testCase1) 10 | // { 11 | // 12 | // } 13 | 14 | typedef iirob_filters::MultiChannelKalmanFilter KalmanFilter; 15 | 16 | TEST(KalmanFilter, testFunctionality) 17 | { 18 | KalmanFilter* filter = new KalmanFilter(); 19 | 20 | // configure default namespace: "KalmanFilter" 21 | EXPECT_TRUE(filter->configure()); 22 | 23 | const int n = 1, m = 1; 24 | 25 | const std::vector measurements = {0.39, 0.50, 0.48, 0.29, 0.25, 0.32, 0.34, 0.48, 0.41, 0.45}; 26 | const std::vector state_estimates = {0., 0.3545454545, 0.4238095238, 0.4419354839, 0.4048780488, 27 | 0.3745098039, 0.3655737705, 0.361971831, 0.3765432099, 0.3802197802}; 28 | const std::vector error_cov_estimates = {1., 0.0909090909, 0.0476190476, 0.0322580645, 29 | 0.0243902439, 0.0196078431, 0.0163934426, 0.014084507, 0.012345679, 0.010989011}; 30 | const std::vector state_updates = {0.3545454545, 0.4238095238, 0.4419354839, 0.4048780488, 31 | 0.3745098039, 0.3655737705, 0.361971831, 0.3765432099, 0.3802197802, 0.3871287129}; 32 | const std::vector error_cov_updates = {0.0909090909, 0.0476190476, 0.0322580645, 33 | 0.0243902439, 0.0196078431, 0.0163934426, 0.014084507, 0.012345679, 0.010989011, 0.0099009901}; 34 | const double eps = 0.0001; 35 | std::vector measurement, state_out; 36 | Eigen::MatrixXd error_cov_out; 37 | 38 | EXPECT_TRUE(filter->getErrorCovarianceMatrix(error_cov_out)); 39 | EXPECT_TRUE(filter->getCurrentState(state_out)); 40 | 41 | for (int i = 0; i < measurements.size(); i++) 42 | { 43 | // configure and check every step 44 | if (i != 0) 45 | { 46 | EXPECT_TRUE(filter->predict(state_out)); 47 | } 48 | 49 | EXPECT_TRUE(state_out.size() == 1); 50 | EXPECT_NEAR (state_out[0], state_estimates[i], eps); 51 | 52 | EXPECT_TRUE(filter->getErrorCovarianceMatrix(error_cov_out)); 53 | EXPECT_TRUE(error_cov_out.size() == 1); 54 | EXPECT_NEAR (error_cov_out(0, 0), error_cov_estimates[i], eps); 55 | 56 | measurement.clear(); measurement.push_back(measurements[i]); 57 | EXPECT_TRUE(filter->update(measurement, state_out)); 58 | EXPECT_TRUE(state_out.size() == 1); 59 | EXPECT_NEAR (state_out[0], state_updates[i], eps); 60 | 61 | EXPECT_TRUE(filter->getErrorCovarianceMatrix(error_cov_out)); 62 | EXPECT_TRUE(error_cov_out.size() == 1); 63 | EXPECT_NEAR (error_cov_out(0, 0), error_cov_updates[i], eps); 64 | 65 | } 66 | } 67 | 68 | TEST(KalmanFilter, testFunctionalityWithMoreDims) 69 | { 70 | KalmanFilter* filter = new KalmanFilter(); 71 | 72 | std::string namespace_ = "KalmanFilterMoreDimensions"; 73 | 74 | EXPECT_TRUE(filter->configure(namespace_)); 75 | } 76 | 77 | TEST(KalmanFilter, testParameters) 78 | { 79 | KalmanFilter* filter = new KalmanFilter(); 80 | 81 | const std::vector namespaces_with_param_errors = {"KalmanFilterTestWithoutParameters", "KalmanFilterTestDt", 82 | "KalmanFilterTestN", "KalmanFilterTestN", 83 | "KalmanFilterTestParameterA", "KalmanFilterTestParameterC", "KalmanFilterTestParameterQ", 84 | "KalmanFilterTestParameterR", "KalmanFilterTestParameterP", "KalmanFilterTestParameterX0" 85 | }; 86 | 87 | for (int i = 0; i < namespaces_with_param_errors.size(); i++) 88 | { 89 | EXPECT_FALSE(filter->configure(namespaces_with_param_errors[i])); 90 | } 91 | } 92 | 93 | // Run all the tests that were declared with TEST() 94 | int main(int argc, char **argv){ 95 | // ROSCONSOLE_AUTOINIT; 96 | // log4cxx::LoggerPtr my_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME); 97 | // my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]); 98 | 99 | testing::InitGoogleTest(&argc, argv); 100 | ros::init(argc, argv, "tester"); 101 | ros::NodeHandle nh; 102 | return RUN_ALL_TESTS(); 103 | } 104 | -------------------------------------------------------------------------------- /test/test_kalman.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /test/test_kalman.yaml: -------------------------------------------------------------------------------- 1 | # Kalman filter params: 2 | # dt="time between two updates" 3 | # n="number of states" 4 | # m="number of measurements" 5 | # A="System dynamics matrix: nxn" 6 | # At="System dynamics matrix (dynamic part): nxn" 7 | # C="Output matrix: mxn" 8 | # Q="Process noise covariance: nxn" 9 | # Q_coeff="coefficients of the dynamic part of Q" 10 | # Q_exponent="exponents of the time differnce" 11 | # Q_variance="variance of the process noise" 12 | # R="Measurement noise covariance: mxm" 13 | # P="Estimate error covariance: nxn" 14 | # x0="start state vector" 15 | 16 | # KalmanFilter="default namespace for parameters" -> configure() can be used without arguments 17 | 18 | # x0 can be empty 19 | # if At is empty or not valid -> update with delta_t is not available 20 | # if Q_coeff is empty or not valid -> Q_exponent and Q_variance are not considered and 21 | # update of the Q matrix is not available 22 | 23 | # default namespace, parameters are set appropriate 24 | KalmanFilter: 25 | dt: 0.01 26 | n: 1 27 | m: 1 28 | # n x n 29 | A: [1] 30 | # m x n 31 | C: [1] 32 | # n x n 33 | Q: [0] 34 | # m x m 35 | R: [0.1] 36 | # n x n 37 | P: [1] 38 | # n 39 | x0: [0] 40 | 41 | KalmanFilterTestWithoutParameters: 42 | hasParameter: false 43 | 44 | KalmanFilterTestDt: 45 | dt: 0 46 | n: 1 47 | m: 1 48 | # n x n 49 | A: [1] 50 | # m x n 51 | C: [1] 52 | # n x n 53 | Q: [0] 54 | # m x m 55 | R: [0.1] 56 | # n x n 57 | P: [1] 58 | # n 59 | x0: [0] 60 | 61 | KalmanFilterTestN: 62 | dt: 0.01 63 | n: 0 64 | m: 1 65 | # n x n 66 | A: [1] 67 | # m x n 68 | C: [1] 69 | # n x n 70 | Q: [0] 71 | # m x m 72 | R: [0.1] 73 | # n x n 74 | P: [1] 75 | # n 76 | x0: [0] 77 | 78 | KalmanFilterTestM: 79 | dt: 0.01 80 | n: 1 81 | m: 0 82 | # n x n 83 | A: [1] 84 | # m x n 85 | C: [1] 86 | # n x n 87 | Q: [0] 88 | # m x m 89 | R: [0.1] 90 | # n x n 91 | P: [1] 92 | # n 93 | x0: [0] 94 | 95 | KalmanFilterTestParameterA: 96 | dt: 0.01 97 | n: 1 98 | m: 1 99 | # n x n 100 | A: [1,1] 101 | # m x n 102 | C: [1] 103 | # n x n 104 | Q: [0] 105 | # m x m 106 | R: [0.1] 107 | # n x n 108 | P: [1] 109 | # n 110 | x0: [0] 111 | 112 | KalmanFilterTestParameterC: 113 | dt: 0.01 114 | n: 1 115 | m: 1 116 | # n x n 117 | A: [1] 118 | # m x n 119 | C: [1,1] 120 | # n x n 121 | Q: [0] 122 | # m x m 123 | R: [0.1] 124 | # n x n 125 | P: [1] 126 | # n 127 | x0: [0] 128 | 129 | KalmanFilterTestParameterQ: 130 | dt: 0.01 131 | n: 1 132 | m: 1 133 | # n x n 134 | A: [1] 135 | # m x n 136 | C: [1] 137 | # n x n 138 | Q: [0,1] 139 | # m x m 140 | R: [0.1] 141 | # n x n 142 | P: [1] 143 | # n 144 | x0: [0] 145 | 146 | KalmanFilterTestParameterR: 147 | dt: 0.01 148 | n: 1 149 | m: 1 150 | # n x n 151 | A: [1] 152 | # m x n 153 | C: [1] 154 | # n x n 155 | Q: [0] 156 | # m x m 157 | R: [0.1,1] 158 | # n x n 159 | P: [1] 160 | # n 161 | x0: [0] 162 | 163 | KalmanFilterTestParameterP: 164 | dt: 0.01 165 | n: 1 166 | m: 1 167 | # n x n 168 | A: [1] 169 | # m x n 170 | C: [1] 171 | # n x n 172 | Q: [0] 173 | # m x m 174 | R: [0.1] 175 | # n x n 176 | P: [1,1] 177 | # n 178 | x0: [0] 179 | 180 | KalmanFilterTestParameterX0: 181 | dt: 0.01 182 | n: 1 183 | m: 1 184 | # n x n 185 | A: [1] 186 | # m x n 187 | C: [1] 188 | # n x n 189 | Q: [0] 190 | # m x m 191 | R: [0.1] 192 | # n x n 193 | P: [1] 194 | # n 195 | x0: [0,1] 196 | 197 | KalmanFilterTestParameterX0Empty: 198 | dt: 0.01 199 | n: 1 200 | m: 1 201 | # n x n 202 | A: [1] 203 | # m x n 204 | C: [1] 205 | # n x n 206 | Q: [0] 207 | # m x m 208 | R: [0.1] 209 | # n x n 210 | P: [1] 211 | # n 212 | x0: [] 213 | 214 | KalmanFilterMoreDimensions: 215 | dt: 0.05 216 | n: 6 217 | m: 2 218 | # n x n 219 | A: [1, 0, 0.05, 0, 0.00125, 0, 220 | 0, 1, 0, 0.05, 0, 0.00125, 221 | 0, 0, 1, 0, 0.05, 0, 222 | 0, 0, 0, 1, 0, 0.05, 223 | 0, 0, 0, 0, 1, 0, 224 | 0, 0, 0, 0, 0, 1] 225 | # m x n 226 | C: [1, 0, 0, 0, 0, 0, 227 | 0, 1, 0, 0, 0, 0] 228 | # n x n 229 | Q: [0.1, 0, 0, 0, 0, 0, 230 | 0, 0.1, 0, 0, 0, 0, 231 | 0, 0, 0.1, 0, 0, 0, 232 | 0, 0, 0, 0.1, 0, 0, 233 | 0, 0, 0, 0, 0.1, 0, 234 | 0, 0, 0, 0, 0, 0.1] 235 | # m x m 236 | 237 | R: [0.0144, 0, 238 | 0, 0.0144] 239 | # n x n 240 | P: [0.45, 0, 0, 0, 0, 0, 241 | 0, 0.45, 0, 0, 0, 0, 242 | 0, 0, 0.45, 0, 0, 0, 243 | 0, 0, 0, 0.45, 0, 0, 244 | 0, 0, 0, 0, 0.45, 0, 245 | 0, 0, 0, 0, 0, 0.45] 246 | # n 247 | x0: [0, 0, 0, 0, 0, 0] 248 | --------------------------------------------------------------------------------