diff options
| -rw-r--r-- | 3rdparty-notices/EIGEN-COPYING.txt | 379 | ||||
| -rw-r--r-- | cmake/FindEigen3.cmake | 106 | ||||
| -rw-r--r-- | filter-kalman/CMakeLists.txt | 5 | ||||
| -rw-r--r-- | filter-kalman/ftnoir_kalman_filtercontrols.ui | 170 | ||||
| -rw-r--r-- | filter-kalman/kalman.cpp | 323 | ||||
| -rw-r--r-- | filter-kalman/kalman.h | 159 | ||||
| -rw-r--r-- | filter-kalman/kalman_simulation.py | 76 | ||||
| -rw-r--r-- | filter-kalman/lang/nl_NL.ts | 38 | ||||
| -rw-r--r-- | filter-kalman/lang/ru_RU.ts | 38 | ||||
| -rw-r--r-- | filter-kalman/lang/stub.ts | 38 | ||||
| -rw-r--r-- | filter-kalman/lang/zh_CN.ts | 38 |
11 files changed, 0 insertions, 1370 deletions
diff --git a/3rdparty-notices/EIGEN-COPYING.txt b/3rdparty-notices/EIGEN-COPYING.txt deleted file mode 100644 index c203fff6..00000000 --- a/3rdparty-notices/EIGEN-COPYING.txt +++ /dev/null @@ -1,379 +0,0 @@ -The opentrack project is using the Eigen3 linear algebra library with site at -<http://eigen.tuxfamily.org/> - -We're only using the MPL2-licensed bits. This is to avoid copyleft. License -follows: - -Mozilla Public License Version 2.0 -================================== - -1. Definitions --------------- - -1.1. "Contributor" - means each individual or legal entity that creates, contributes to - the creation of, or owns Covered Software. - -1.2. "Contributor Version" - means the combination of the Contributions of others (if any) used - by a Contributor and that particular Contributor's Contribution. - -1.3. "Contribution" - means Covered Software of a particular Contributor. - -1.4. "Covered Software" - means Source Code Form to which the initial Contributor has attached - the notice in Exhibit A, the Executable Form of such Source Code - Form, and Modifications of such Source Code Form, in each case - including portions thereof. - -1.5. "Incompatible With Secondary Licenses" - means - - (a) that the initial Contributor has attached the notice described - in Exhibit B to the Covered Software; or - - (b) that the Covered Software was made available under the terms of - version 1.1 or earlier of the License, but not also under the - terms of a Secondary License. - -1.6. "Executable Form" - means any form of the work other than Source Code Form. - -1.7. "Larger Work" - means a work that combines Covered Software with other material, in - a separate file or files, that is not Covered Software. - -1.8. "License" - means this document. - -1.9. "Licensable" - means having the right to grant, to the maximum extent possible, - whether at the time of the initial grant or subsequently, any and - all of the rights conveyed by this License. - -1.10. "Modifications" - means any of the following: - - (a) any file in Source Code Form that results from an addition to, - deletion from, or modification of the contents of Covered - Software; or - - (b) any new file in Source Code Form that contains any Covered - Software. - -1.11. "Patent Claims" of a Contributor - means any patent claim(s), including without limitation, method, - process, and apparatus claims, in any patent Licensable by such - Contributor that would be infringed, but for the grant of the - License, by the making, using, selling, offering for sale, having - made, import, or transfer of either its Contributions or its - Contributor Version. - -1.12. "Secondary License" - means either the GNU General Public License, Version 2.0, the GNU - Lesser General Public License, Version 2.1, the GNU Affero General - Public License, Version 3.0, or any later versions of those - licenses. - -1.13. "Source Code Form" - means the form of the work preferred for making modifications. - -1.14. "You" (or "Your") - means an individual or a legal entity exercising rights under this - License. For legal entities, "You" includes any entity that - controls, is controlled by, or is under common control with You. For - purposes of this definition, "control" means (a) the power, direct - or indirect, to cause the direction or management of such entity, - whether by contract or otherwise, or (b) ownership of more than - fifty percent (50%) of the outstanding shares or beneficial - ownership of such entity. - -2. License Grants and Conditions --------------------------------- - -2.1. Grants - -Each Contributor hereby grants You a world-wide, royalty-free, -non-exclusive license: - -(a) under intellectual property rights (other than patent or trademark) - Licensable by such Contributor to use, reproduce, make available, - modify, display, perform, distribute, and otherwise exploit its - Contributions, either on an unmodified basis, with Modifications, or - as part of a Larger Work; and - -(b) under Patent Claims of such Contributor to make, use, sell, offer - for sale, have made, import, and otherwise transfer either its - Contributions or its Contributor Version. - -2.2. Effective Date - -The licenses granted in Section 2.1 with respect to any Contribution -become effective for each Contribution on the date the Contributor first -distributes such Contribution. - -2.3. Limitations on Grant Scope - -The licenses granted in this Section 2 are the only rights granted under -this License. No additional rights or licenses will be implied from the -distribution or licensing of Covered Software under this License. -Notwithstanding Section 2.1(b) above, no patent license is granted by a -Contributor: - -(a) for any code that a Contributor has removed from Covered Software; - or - -(b) for infringements caused by: (i) Your and any other third party's - modifications of Covered Software, or (ii) the combination of its - Contributions with other software (except as part of its Contributor - Version); or - -(c) under Patent Claims infringed by Covered Software in the absence of - its Contributions. - -This License does not grant any rights in the trademarks, service marks, -or logos of any Contributor (except as may be necessary to comply with -the notice requirements in Section 3.4). - -2.4. Subsequent Licenses - -No Contributor makes additional grants as a result of Your choice to -distribute the Covered Software under a subsequent version of this -License (see Section 10.2) or under the terms of a Secondary License (if -permitted under the terms of Section 3.3). - -2.5. Representation - -Each Contributor represents that the Contributor believes its -Contributions are its original creation(s) or it has sufficient rights -to grant the rights to its Contributions conveyed by this License. - -2.6. Fair Use - -This License is not intended to limit any rights You have under -applicable copyright doctrines of fair use, fair dealing, or other -equivalents. - -2.7. Conditions - -Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted -in Section 2.1. - -3. Responsibilities -------------------- - -3.1. Distribution of Source Form - -All distribution of Covered Software in Source Code Form, including any -Modifications that You create or to which You contribute, must be under -the terms of this License. You must inform recipients that the Source -Code Form of the Covered Software is governed by the terms of this -License, and how they can obtain a copy of this License. You may not -attempt to alter or restrict the recipients' rights in the Source Code -Form. - -3.2. Distribution of Executable Form - -If You distribute Covered Software in Executable Form then: - -(a) such Covered Software must also be made available in Source Code - Form, as described in Section 3.1, and You must inform recipients of - the Executable Form how they can obtain a copy of such Source Code - Form by reasonable means in a timely manner, at a charge no more - than the cost of distribution to the recipient; and - -(b) You may distribute such Executable Form under the terms of this - License, or sublicense it under different terms, provided that the - license for the Executable Form does not attempt to limit or alter - the recipients' rights in the Source Code Form under this License. - -3.3. Distribution of a Larger Work - -You may create and distribute a Larger Work under terms of Your choice, -provided that You also comply with the requirements of this License for -the Covered Software. If the Larger Work is a combination of Covered -Software with a work governed by one or more Secondary Licenses, and the -Covered Software is not Incompatible With Secondary Licenses, this -License permits You to additionally distribute such Covered Software -under the terms of such Secondary License(s), so that the recipient of -the Larger Work may, at their option, further distribute the Covered -Software under the terms of either this License or such Secondary -License(s). - -3.4. Notices - -You may not remove or alter the substance of any license notices -(including copyright notices, patent notices, disclaimers of warranty, -or limitations of liability) contained within the Source Code Form of -the Covered Software, except that You may alter any license notices to -the extent required to remedy known factual inaccuracies. - -3.5. Application of Additional Terms - -You may choose to offer, and to charge a fee for, warranty, support, -indemnity or liability obligations to one or more recipients of Covered -Software. However, You may do so only on Your own behalf, and not on -behalf of any Contributor. You must make it absolutely clear that any -such warranty, support, indemnity, or liability obligation is offered by -You alone, and You hereby agree to indemnify every Contributor for any -liability incurred by such Contributor as a result of warranty, support, -indemnity or liability terms You offer. You may include additional -disclaimers of warranty and limitations of liability specific to any -jurisdiction. - -4. Inability to Comply Due to Statute or Regulation ---------------------------------------------------- - -If it is impossible for You to comply with any of the terms of this -License with respect to some or all of the Covered Software due to -statute, judicial order, or regulation then You must: (a) comply with -the terms of this License to the maximum extent possible; and (b) -describe the limitations and the code they affect. Such description must -be placed in a text file included with all distributions of the Covered -Software under this License. Except to the extent prohibited by statute -or regulation, such description must be sufficiently detailed for a -recipient of ordinary skill to be able to understand it. - -5. Termination --------------- - -5.1. The rights granted under this License will terminate automatically -if You fail to comply with any of its terms. However, if You become -compliant, then the rights granted under this License from a particular -Contributor are reinstated (a) provisionally, unless and until such -Contributor explicitly and finally terminates Your grants, and (b) on an -ongoing basis, if such Contributor fails to notify You of the -non-compliance by some reasonable means prior to 60 days after You have -come back into compliance. Moreover, Your grants from a particular -Contributor are reinstated on an ongoing basis if such Contributor -notifies You of the non-compliance by some reasonable means, this is the -first time You have received notice of non-compliance with this License -from such Contributor, and You become compliant prior to 30 days after -Your receipt of the notice. - -5.2. If You initiate litigation against any entity by asserting a patent -infringement claim (excluding declaratory judgment actions, -counter-claims, and cross-claims) alleging that a Contributor Version -directly or indirectly infringes any patent, then the rights granted to -You by any and all Contributors for the Covered Software under Section -2.1 of this License shall terminate. - -5.3. In the event of termination under Sections 5.1 or 5.2 above, all -end user license agreements (excluding distributors and resellers) which -have been validly granted by You or Your distributors under this License -prior to termination shall survive termination. - -************************************************************************ -* * -* 6. Disclaimer of Warranty * -* ------------------------- * -* * -* Covered Software is provided under this License on an "as is" * -* basis, without warranty of any kind, either expressed, implied, or * -* statutory, including, without limitation, warranties that the * -* Covered Software is free of defects, merchantable, fit for a * -* particular purpose or non-infringing. The entire risk as to the * -* quality and performance of the Covered Software is with You. * -* Should any Covered Software prove defective in any respect, You * -* (not any Contributor) assume the cost of any necessary servicing, * -* repair, or correction. This disclaimer of warranty constitutes an * -* essential part of this License. No use of any Covered Software is * -* authorized under this License except under this disclaimer. * -* * -************************************************************************ - -************************************************************************ -* * -* 7. Limitation of Liability * -* -------------------------- * -* * -* Under no circumstances and under no legal theory, whether tort * -* (including negligence), contract, or otherwise, shall any * -* Contributor, or anyone who distributes Covered Software as * -* permitted above, be liable to You for any direct, indirect, * -* special, incidental, or consequential damages of any character * -* including, without limitation, damages for lost profits, loss of * -* goodwill, work stoppage, computer failure or malfunction, or any * -* and all other commercial damages or losses, even if such party * -* shall have been informed of the possibility of such damages. This * -* limitation of liability shall not apply to liability for death or * -* personal injury resulting from such party's negligence to the * -* extent applicable law prohibits such limitation. Some * -* jurisdictions do not allow the exclusion or limitation of * -* incidental or consequential damages, so this exclusion and * -* limitation may not apply to You. * -* * -************************************************************************ - -8. Litigation -------------- - -Any litigation relating to this License may be brought only in the -courts of a jurisdiction where the defendant maintains its principal -place of business and such litigation shall be governed by laws of that -jurisdiction, without reference to its conflict-of-law provisions. -Nothing in this Section shall prevent a party's ability to bring -cross-claims or counter-claims. - -9. Miscellaneous ----------------- - -This License represents the complete agreement concerning the subject -matter hereof. If any provision of this License is held to be -unenforceable, such provision shall be reformed only to the extent -necessary to make it enforceable. Any law or regulation which provides -that the language of a contract shall be construed against the drafter -shall not be used to construe this License against a Contributor. - -10. Versions of the License ---------------------------- - -10.1. New Versions - -Mozilla Foundation is the license steward. Except as provided in Section -10.3, no one other than the license steward has the right to modify or -publish new versions of this License. Each version will be given a -distinguishing version number. - -10.2. Effect of New Versions - -You may distribute the Covered Software under the terms of the version -of the License under which You originally received the Covered Software, -or under the terms of any subsequent version published by the license -steward. - -10.3. Modified Versions - -If you create software not governed by this License, and you want to -create a new license for such software, you may create and use a -modified version of this License if you rename the license and remove -any references to the name of the license steward (except to note that -such modified license differs from this License). - -10.4. Distributing Source Code Form that is Incompatible With Secondary -Licenses - -If You choose to distribute Source Code Form that is Incompatible With -Secondary Licenses under the terms of this version of the License, the -notice described in Exhibit B of this License must be attached. - -Exhibit A - Source Code Form License Notice -------------------------------------------- - - This Source Code Form is subject to the terms of the Mozilla Public - License, v. 2.0. If a copy of the MPL was not distributed with this - file, You can obtain one at http://mozilla.org/MPL/2.0/. - -If it is not possible or desirable to put the notice in a particular -file, then You may include the notice in a location (such as a LICENSE -file in a relevant directory) where a recipient would be likely to look -for such a notice. - -You may add additional accurate notices of copyright ownership. - -Exhibit B - "Incompatible With Secondary Licenses" Notice ---------------------------------------------------------- - - This Source Code Form is "Incompatible With Secondary Licenses", as - defined by the Mozilla Public License, v. 2.0. diff --git a/cmake/FindEigen3.cmake b/cmake/FindEigen3.cmake deleted file mode 100644 index bbad2298..00000000 --- a/cmake/FindEigen3.cmake +++ /dev/null @@ -1,106 +0,0 @@ -# - Try to find Eigen3 lib -# -# This module supports requiring a minimum version, e.g. you can do -# find_package(Eigen3 3.1.2) -# to require version 3.1.2 or newer of Eigen3. -# -# Once done this will define -# -# EIGEN3_FOUND - system has eigen lib with correct version -# EIGEN3_INCLUDE_DIR - the eigen include directory -# EIGEN3_VERSION - eigen version -# -# This module reads hints about search locations from -# the following enviroment variables: -# -# EIGEN3_ROOT -# EIGEN3_ROOT_DIR - -# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org> -# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr> -# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com> -# Redistribution and use is allowed according to the terms of the 2-clause BSD license. - -if(NOT Eigen3_FIND_VERSION) - if(NOT Eigen3_FIND_VERSION_MAJOR) - set(Eigen3_FIND_VERSION_MAJOR 2) - endif(NOT Eigen3_FIND_VERSION_MAJOR) - if(NOT Eigen3_FIND_VERSION_MINOR) - set(Eigen3_FIND_VERSION_MINOR 91) - endif(NOT Eigen3_FIND_VERSION_MINOR) - if(NOT Eigen3_FIND_VERSION_PATCH) - set(Eigen3_FIND_VERSION_PATCH 0) - endif(NOT Eigen3_FIND_VERSION_PATCH) - - set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") -endif(NOT Eigen3_FIND_VERSION) - -macro(_eigen3_check_version) - file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) - - string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") - set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") - set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") - set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") - - set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) - if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - set(EIGEN3_VERSION_OK FALSE) - else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - set(EIGEN3_VERSION_OK TRUE) - endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - - if(NOT EIGEN3_VERSION_OK) - - message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " - "but at least version ${Eigen3_FIND_VERSION} is required") - endif(NOT EIGEN3_VERSION_OK) -endmacro(_eigen3_check_version) - -if (EIGEN3_INCLUDE_DIR) - - # in cache already - _eigen3_check_version() - set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) - -else (EIGEN3_INCLUDE_DIR) - - # search first if an Eigen3Config.cmake is available in the system, - # if successful this would set EIGEN3_INCLUDE_DIR and the rest of - # the script will work as usual - find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET) - - if(NOT EIGEN3_INCLUDE_DIR) - find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library - HINTS - ENV EIGEN3_ROOT - ENV EIGEN3_ROOT_DIR - PATHS - ${CMAKE_INSTALL_PREFIX}/include - ${KDE4_INCLUDE_DIR} - PATH_SUFFIXES eigen3 eigen - ) - endif(NOT EIGEN3_INCLUDE_DIR) - - if(EIGEN3_INCLUDE_DIR) - _eigen3_check_version() - endif(EIGEN3_INCLUDE_DIR) - - include(FindPackageHandleStandardArgs) - find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) - - mark_as_advanced(EIGEN3_INCLUDE_DIR) - -endif(EIGEN3_INCLUDE_DIR) - -if(EIGEN3_FOUND) - add_definitions( - -DEIGEN_MPL2_ONLY=1 - -DEIGEN_MAX_CPP_VER=17 - -DEIGEN_DEFAULT_DENSE_INDEX_TYPE=int - -DEIGEN_NO_DEBUG=1 - -DEIGEN_DONT_PARALLELIZE=1 - ) -endif() diff --git a/filter-kalman/CMakeLists.txt b/filter-kalman/CMakeLists.txt deleted file mode 100644 index b6f8100e..00000000 --- a/filter-kalman/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -find_package(Eigen3 QUIET) -if(EIGEN3_FOUND) - otr_module(filter-kalman) - target_include_directories(opentrack-filter-kalman SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR}) -endif() diff --git a/filter-kalman/ftnoir_kalman_filtercontrols.ui b/filter-kalman/ftnoir_kalman_filtercontrols.ui deleted file mode 100644 index 83ee8e96..00000000 --- a/filter-kalman/ftnoir_kalman_filtercontrols.ui +++ /dev/null @@ -1,170 +0,0 @@ -<?xml version="1.0" encoding="UTF-8"?> -<ui version="4.0"> - <class>KalmanUICdialog_kalman</class> - <widget class="QWidget" name="KalmanUICdialog_kalman"> - <property name="windowModality"> - <enum>Qt::NonModal</enum> - </property> - <property name="geometry"> - <rect> - <x>0</x> - <y>0</y> - <width>438</width> - <height>141</height> - </rect> - </property> - <property name="sizePolicy"> - <sizepolicy hsizetype="Fixed" vsizetype="Fixed"> - <horstretch>0</horstretch> - <verstretch>0</verstretch> - </sizepolicy> - </property> - <property name="windowTitle"> - <string>Kalman settings</string> - </property> - <property name="windowIcon"> - <iconset resource="../gui/opentrack-res.qrc"> - <normaloff>:/images/filter-16.png</normaloff>:/images/filter-16.png</iconset> - </property> - <property name="layoutDirection"> - <enum>Qt::LeftToRight</enum> - </property> - <property name="autoFillBackground"> - <bool>false</bool> - </property> - <property name="styleSheet"> - <string notr="true"/> - </property> - <layout class="QVBoxLayout" name="verticalLayout"> - <item> - <widget class="QGroupBox" name="groupBox_2"> - <property name="title"> - <string>Measurement noise</string> - </property> - <layout class="QGridLayout" name="gridLayout_2"> - <item row="0" column="0"> - <widget class="QLabel" name="label"> - <property name="text"> - <string>Rotation</string> - </property> - </widget> - </item> - <item row="0" column="1"> - <widget class="QSlider" name="noiseRotSlider"> - <property name="maximum"> - <number>400</number> - </property> - <property name="singleStep"> - <number>1</number> - </property> - <property name="pageStep"> - <number>100</number> - </property> - <property name="tracking"> - <bool>true</bool> - </property> - <property name="orientation"> - <enum>Qt::Horizontal</enum> - </property> - <property name="invertedAppearance"> - <bool>false</bool> - </property> - <property name="tickPosition"> - <enum>QSlider::TicksBelow</enum> - </property> - <property name="tickInterval"> - <number>100</number> - </property> - </widget> - </item> - <item row="1" column="0"> - <widget class="QLabel" name="label_4"> - <property name="text"> - <string>Position</string> - </property> - </widget> - </item> - <item row="1" column="1"> - <widget class="QSlider" name="noisePosSlider"> - <property name="maximum"> - <number>400</number> - </property> - <property name="pageStep"> - <number>100</number> - </property> - <property name="orientation"> - <enum>Qt::Horizontal</enum> - </property> - <property name="tickPosition"> - <enum>QSlider::TicksBelow</enum> - </property> - <property name="tickInterval"> - <number>100</number> - </property> - </widget> - </item> - <item row="0" column="2"> - <widget class="QLabel" name="noiseRotLabel"> - <property name="minimumSize"> - <size> - <width>65</width> - <height>0</height> - </size> - </property> - <property name="text"> - <string>°</string> - </property> - </widget> - </item> - <item row="1" column="2"> - <widget class="QLabel" name="noisePosLabel"> - <property name="minimumSize"> - <size> - <width>65</width> - <height>0</height> - </size> - </property> - <property name="text"> - <string>-</string> - </property> - </widget> - </item> - </layout> - </widget> - </item> - <item> - <widget class="QDialogButtonBox" name="buttonBox"> - <property name="standardButtons"> - <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set> - </property> - </widget> - </item> - </layout> - </widget> - <resources> - <include location="../gui/opentrack-res.qrc"/> - </resources> - <connections/> - <designerdata> - <property name="gridDeltaX"> - <number>10</number> - </property> - <property name="gridDeltaY"> - <number>10</number> - </property> - <property name="gridSnapX"> - <bool>false</bool> - </property> - <property name="gridSnapY"> - <bool>false</bool> - </property> - <property name="gridVisible"> - <bool>true</bool> - </property> - </designerdata> - <slots> - <slot>startEngineClicked()</slot> - <slot>stopEngineClicked()</slot> - <slot>cameraSettingsClicked()</slot> - </slots> -</ui> diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp deleted file mode 100644 index 1960e0f6..00000000 --- a/filter-kalman/kalman.cpp +++ /dev/null @@ -1,323 +0,0 @@ -/* Copyright (c) 2016 Michael Welter <mw.pub@welter-4d.de> - * - * Permission to use, copy, modify, and/or distribute this software for any - * purpose with or without fee is hereby granted, provided that the above - * copyright notice and this permission notice appear in all copies. - */ -#include "kalman.h" -#include <cmath> -#include <QDebug> - -void KalmanFilter::init() -{ - // allocate and initialize matrices - measurement_noise_cov = MeasureMatrix::Zero(); - process_noise_cov = StateMatrix::Zero(); - state_cov = StateMatrix::Zero(); - state_cov_prior = StateMatrix::Zero(); - transition_matrix = StateMatrix::Zero(); - measurement_matrix = StateToMeasureMatrix::Zero(); - kalman_gain = MeasureToStateMatrix::Zero(); - // initialize state variables - state = StateVector::Zero(); - state_prior = StateVector::Zero(); - innovation = PoseVector::Zero(); -} - - -void KalmanFilter::time_update() -{ - state_prior = transition_matrix * state; - state_cov_prior = transition_matrix * state_cov * transition_matrix.transpose() + process_noise_cov; -} - - -void KalmanFilter::measurement_update(const PoseVector &measurement) -{ - MeasureMatrix tmp = measurement_matrix * state_cov_prior * measurement_matrix.transpose() + measurement_noise_cov; - MeasureMatrix tmp_inv = tmp.inverse(); - kalman_gain = state_cov_prior * measurement_matrix.transpose() * tmp_inv; - innovation = measurement - measurement_matrix * state_prior; - state = state_prior + kalman_gain * innovation; - state_cov = state_cov_prior - kalman_gain * measurement_matrix * state_cov_prior; -} - - - -void KalmanProcessNoiseScaler::init() -{ - base_cov = StateMatrix::Zero(NUM_STATE_DOF, NUM_STATE_DOF); - innovation_cov_estimate = MeasureMatrix::Zero(NUM_MEASUREMENT_DOF, NUM_MEASUREMENT_DOF); -} - - -/* Uses - innovation, measurement_matrix, measurement_noise_cov, and state_cov_prior - found in KalmanFilter. It sets - process_noise_cov -*/ -void KalmanProcessNoiseScaler::update(KalmanFilter &kf, double dt) -{ - MeasureMatrix ddT = kf.innovation * kf.innovation.transpose(); - double f = dt / (dt + settings::adaptivity_window_length); - innovation_cov_estimate = - f * ddT + (1. - f) * innovation_cov_estimate; - - double T1 = (innovation_cov_estimate - kf.measurement_noise_cov).trace(); - double T2 = (kf.measurement_matrix * kf.state_cov_prior * kf.measurement_matrix.transpose()).trace(); - double alpha = 0.001; - if (T2 > 0. && T1 > 0.) - { - alpha = T1 / T2; - alpha = std::sqrt(alpha); - alpha = std::fmin(1000., std::fmax(0.001, alpha)); - } - kf.process_noise_cov = alpha * base_cov; - //qDebug() << "alpha = " << alpha; -} - -void DeadzoneFilter::reset() -{ - last_output = PoseVector::Zero(); -} - -PoseVector DeadzoneFilter::filter(const PoseVector &input) -{ - PoseVector out; - for (int i = 0; i < input.rows(); ++i) - { - const double dz = dz_size[i]; - if (dz > 0.) - { - const double delta = input[i] - last_output[i]; - const double f = std::pow(std::fabs(delta) / dz, settings::deadzone_exponent); - const double response = f / (f + 1.) * delta; - out[i] = last_output[i] + response; - } - else - out[i] = input[i]; - last_output[i] = out[i]; - } - return out; -} - - -void kalman::fill_transition_matrix(double dt) -{ - for (int i = 0; i < 6; ++i) - { - kf.transition_matrix(i, i + 6) = dt; - } -} - -void kalman::fill_process_noise_cov_matrix(StateMatrix &target, double dt) const -{ - // This model is like movement at fixed velocity plus superimposed - // brownian motion. Unlike standard models for tracking of objects - // with a very well predictable trajectory (e.g. - // https://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical) - double sigma_pos = settings::process_sigma_pos; - double sigma_angle = settings::process_sigma_rot; - double a_pos = sigma_pos * sigma_pos * dt; - double a_ang = sigma_angle * sigma_angle * dt; - constexpr double b = 20; - constexpr double c = 1.; - for (int i = 0; i < 3; ++i) - { - target(i, i) = a_pos; - target(i, i + 6) = a_pos * c; - target(i + 6, i) = a_pos * c; - target(i + 6, i + 6) = a_pos * b; - } - for (int i = 3; i < 6; ++i) - { - target(i, i) = a_ang; - target(i, i + 6) = a_ang * c; - target(i + 6, i) = a_ang * c; - target(i + 6, i + 6) = a_ang * b; - } -} - - -PoseVector kalman::do_kalman_filter(const PoseVector &input, double dt, bool new_input) -{ - if (new_input) - { - dt = dt_since_last_input; - fill_transition_matrix(dt); - fill_process_noise_cov_matrix(kf_adaptive_process_noise_cov.base_cov, dt); - kf_adaptive_process_noise_cov.update(kf, dt); - kf.time_update(); - kf.measurement_update(input); - } - return kf.state.head(6); -} - - - -kalman::kalman() -{ - reset(); -} - -// The original code was written by Donovan Baarda <abo@minkirri.apana.org.au> -// https://sourceforge.net/p/facetracknoir/discussion/1150909/thread/418615e1/?limit=25#af75/084b -void kalman::reset() -{ - kf.init(); - kf_adaptive_process_noise_cov.init(); - for (int i = 0; i < 6; ++i) - { - // initialize part of the transition matrix that do not change. - kf.transition_matrix(i, i) = 1.; - kf.transition_matrix(i + 6, i + 6) = 1.; - // "extract" positions, i.e. the first 6 state dof. - kf.measurement_matrix(i, i) = 1.; - } - - double noise_variance_position = settings::map_slider_value(s.noise_pos_slider_value); - double noise_variance_angle = settings::map_slider_value(s.noise_rot_slider_value); - for (int i = 0; i < 3; ++i) - { - kf.measurement_noise_cov(i , i ) = noise_variance_position; - kf.measurement_noise_cov(i + 3, i + 3) = noise_variance_angle; - } - - fill_transition_matrix(0.03); - fill_process_noise_cov_matrix(kf_adaptive_process_noise_cov.base_cov, 0.03); - - kf.process_noise_cov = kf_adaptive_process_noise_cov.base_cov; - kf.state_cov = kf.process_noise_cov; - - for (int i = 0; i < 6; i++) { - last_input[i] = 0; - } - dt_since_last_input = 0; - - prev_slider_pos[0] = s.noise_pos_slider_value; - prev_slider_pos[1] = s.noise_rot_slider_value; - - dz_filter.reset(); -} - - -void kalman::filter(const double* input_, double *output_) -{ - // almost non-existent cost, so might as well ... - Eigen::Map<const PoseVector> input(input_, PoseVector::RowsAtCompileTime, 1); - Eigen::Map<PoseVector> output(output_, PoseVector::RowsAtCompileTime, 1); - - if (!(prev_slider_pos[0] == s.noise_pos_slider_value && - prev_slider_pos[1] == s.noise_rot_slider_value)) - { - reset(); - } - - // Start the timer on first filter evaluation. - if (first_run) - { - timer.start(); - first_run = false; - return; - } - - // Note this is a terrible way to detect when there is a new - // frame of tracker input, but it is the best we have. - bool new_input = input.cwiseNotEqual(last_input).any(); - - // Get the time in seconds since last run and restart the timer. - const double dt = timer.elapsed_seconds(); - dt_since_last_input += dt; - timer.start(); - - output = do_kalman_filter(input, dt, new_input); - - { - // Compute deadzone size base on estimated state variance. - // Given a constant input plus measurement noise, KF should converge to the true input. - // This works well. That is the output pose becomes very still afte some time. - // The QScaling adaptive filter makes the state cov vary depending on the estimated noise - // and the measured noise of the innovation sequence. After a sudden movement it peaks - // and then decays asymptotically to some constant value taken in stationary state. - // We can use this to calculate the size of the deadzone, so that in the stationary state the - // deadzone size is small. Thus the tracking error due to the dz-filter becomes also small. - PoseVector variance = kf.state_cov.diagonal().head(6); - dz_filter.dz_size = variance.cwiseSqrt() * settings::deadzone_scale; - } - output = dz_filter.filter(output); - - if (new_input) - { - dt_since_last_input = 0; - last_input = input; - } -} - - - -dialog_kalman::dialog_kalman() - : filter(nullptr) -{ - ui.setupUi(this); - connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); - connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); - - tie_setting(s.noise_rot_slider_value, ui.noiseRotSlider); - tie_setting(s.noise_pos_slider_value, ui.noisePosSlider); - - connect(&s.noise_rot_slider_value, SIGNAL(valueChanged(const slider_value&)), this, SLOT(updateLabels(const slider_value&))); - connect(&s.noise_pos_slider_value, SIGNAL(valueChanged(const slider_value&)), this, SLOT(updateLabels(const slider_value&))); - - updateLabels(slider_value()); -} - - -void dialog_kalman::updateLabels(const slider_value&) -{ - this->ui.noiseRotLabel->setText( - QString::number(settings::map_slider_value(s.noise_rot_slider_value), 'f', 3) + "°"); - - this->ui.noisePosLabel->setText( - QString::number(settings::map_slider_value(s.noise_pos_slider_value), 'f', 3) + " cm"); -} - - -void dialog_kalman::doOK() { - s.b->save(); - close(); -} - - -void dialog_kalman::doCancel() -{ - close(); -} - -double settings::map_slider_value(const slider_value& v_) -{ - const double v = v_; -#if 0 - //return std::pow(10., v * 4. - 3.); -#else - constexpr int min_log10 = -3; - constexpr int max_log10 = 1; - constexpr int num_divisions = max_log10 - min_log10; - /* ascii art representation of slider - // ----- // ------// ------// ------- // 4 divisions - -3 - 2 -1 0 1 power of 10 - | | - | f + left_side_log10 - | - left_side_log10 - */ - const int k = int(v * num_divisions); // in which division are we?! - const double f = v * num_divisions - k; // where in the division are we?! - const double ff = f * 9. + 1.; - const double multiplier = int(ff * 10.) / 10.; - const int left_side_log10 = min_log10 + k; - const double val = std::pow(10., left_side_log10) * multiplier; - return val; -#endif -} - -OPENTRACK_DECLARE_FILTER(kalman, dialog_kalman, kalmanDll) diff --git a/filter-kalman/kalman.h b/filter-kalman/kalman.h deleted file mode 100644 index 1e6e2ac4..00000000 --- a/filter-kalman/kalman.h +++ /dev/null @@ -1,159 +0,0 @@ -#pragma once -/* Copyright (c) 2016 Michael Welter <mw.pub@welter-4d.de> - * - * Permission to use, copy, modify, and/or distribute this software for any - * purpose with or without fee is hereby granted, provided that the above - * copyright notice and this permission notice appear in all copies. - */ - -#include "compat/timer.hpp" -#include "api/plugin-api.hpp" -#include "options/options.hpp" -using namespace options; - -// Eigen can't check for SSE3 on MSVC -#if defined _MSC_VER && defined __SSE2__ -# define EIGEN_VECTORIZE_SSE3 -// this hardware is 10 years old -# define EIGEN_VECTORIZE_SSE4_1 -# define EIGEN_VECTORIZE_SSE4_2 -#endif - -// nodiscard for placement new -#ifdef _MSC_VER -# pragma warning(push) -# pragma warning(disable : 4834) -#endif - -#include <Eigen/Core> -#include <Eigen/LU> - -#ifdef _MSC_VER -# pragma warning(pop) -#endif - -#include "ui_ftnoir_kalman_filtercontrols.h" -#include <QString> -#include <QWidget> - -static constexpr int NUM_STATE_DOF = 12; -static constexpr int NUM_MEASUREMENT_DOF = 6; -// These vectors are compile time fixed size, stack allocated -using StateToMeasureMatrix = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, NUM_STATE_DOF>; -using StateMatrix = Eigen::Matrix<double, NUM_STATE_DOF, NUM_STATE_DOF>; -using MeasureToStateMatrix = Eigen::Matrix<double, NUM_STATE_DOF, NUM_MEASUREMENT_DOF>; -using MeasureMatrix = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, NUM_MEASUREMENT_DOF>; -using StateVector = Eigen::Matrix<double, NUM_STATE_DOF, 1>; -using PoseVector = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, 1>; - -struct KalmanFilter -{ - MeasureMatrix - measurement_noise_cov; - StateMatrix - process_noise_cov, - state_cov, - state_cov_prior, - transition_matrix; - MeasureToStateMatrix - kalman_gain; - StateToMeasureMatrix - measurement_matrix; - StateVector - state, - state_prior; - PoseVector - innovation; - - void init(); - void time_update(); - void measurement_update(const PoseVector &measurement); -}; - -struct KalmanProcessNoiseScaler -{ - MeasureMatrix - innovation_cov_estimate; - StateMatrix - base_cov; // baseline (unscaled) process noise covariance matrix - void init(); - void update(KalmanFilter &kf, double dt); -}; - - -struct DeadzoneFilter -{ - PoseVector last_output { PoseVector::Zero() }, - dz_size { PoseVector::Zero() }; - - DeadzoneFilter() = default; - void reset(); - PoseVector filter(const PoseVector &input); -}; - - -struct settings : opts { - value<slider_value> noise_rot_slider_value { b, "noise-rotation-slider", { .5, 0, 1 } }; - value<slider_value> noise_pos_slider_value { b, "noise-position-slider", { .5, 0, 1 } }; - - static constexpr double adaptivity_window_length = 0.25; // seconds - static constexpr double deadzone_scale = 8; - static constexpr double deadzone_exponent = 2.0; - static constexpr double process_sigma_pos = 0.5; - static constexpr double process_sigma_rot = 0.5; - - static double map_slider_value(const slider_value &v); - - settings() : opts("kalman-filter") {} -}; - -class kalman : public IFilter -{ - PoseVector do_kalman_filter(const PoseVector &input, double dt, bool new_input); - void fill_transition_matrix(double dt); - void fill_process_noise_cov_matrix(StateMatrix &target, double dt) const; -public: - kalman(); - void reset(); - void filter(const double *input, double *output) override; - void center() override { reset(); } - module_status initialize() override { return status_ok(); } - - double dt_since_last_input; - PoseVector last_input; - KalmanFilter kf; - KalmanProcessNoiseScaler kf_adaptive_process_noise_cov; - DeadzoneFilter dz_filter; - settings s; - slider_value prev_slider_pos[2] { - *s.noise_pos_slider_value, - *s.noise_rot_slider_value, - }; - Timer timer; - - bool first_run = true; -}; - -class kalmanDll : public Metadata -{ - Q_OBJECT - - QString name() override { return tr("Kalman"); } - QIcon icon() override { return QIcon(":/images/filter-16.png"); } -}; - -class dialog_kalman: public IFilterDialog -{ - Q_OBJECT -public: - dialog_kalman(); - Ui::KalmanUICdialog_kalman ui; - void register_filter(IFilter*) override {} - void unregister_filter() override {} - settings s; - kalman *filter; -public slots: - void doOK(); - void doCancel(); - void updateLabels(const slider_value&); -}; diff --git a/filter-kalman/kalman_simulation.py b/filter-kalman/kalman_simulation.py deleted file mode 100644 index 3e792212..00000000 --- a/filter-kalman/kalman_simulation.py +++ /dev/null @@ -1,76 +0,0 @@ -# -*- coding: utf-8 -*- -import numpy as np -import numpy.matlib as mt -import matplotlib.pyplot as plt - -M = 2 -N = 500 -dt = 1.0 / 30. -sigma_measure = 0.1 -# x = 1/2 a t**2 -# assume x = 0.5 in 0.5, i.e. head turns half maximal movement range in 0.5 sec. -# so a = 2 x / t**2 = 4.0 -sigma_accel = 4.0 - -x = np.zeros(N, dtype = np.float) -x[N/3:] = 1.0 # step input -measurement = x.copy() -measurement += np.random.normal(0., sigma_measure, N) - -A = np.matrix([ - [ 1. , dt ], - [ 0. , 1. ] -]) - -R = np.matrix([[ sigma_measure**2 ]]) - -dv = sigma_accel * dt -dp = sigma_accel * 0.5 * dt * dt -Q = np.matrix([ - [ dp*dp, dp*dv ], - [ dv*dp, dv*dv ] -]) - -H = np.matrix([ - [ 1., 0. ], -]) - -I = mt.identity(M, dtype = np.float) - -def arrayOfMatrices(n, shape): - return np.asarray([mt.zeros(shape, dtype = np.float) for i in xrange(n)]) - -# Base on the scipy-cookbook http://scipy-cookbook.readthedocs.io/items/KalmanFiltering.html -sz_state = (M, 1) -sz_cov = (M, M) -sz_K = (M, 1) -xhat= arrayOfMatrices(N, sz_state) # a posteri estimate of x -P=arrayOfMatrices(N, sz_cov) # a posteri error estimate -xhatminus=arrayOfMatrices(N, sz_state) # a priori estimate of x -Pminus=arrayOfMatrices(N, sz_cov) # a priori error estimate -K=arrayOfMatrices(N, sz_K) # gain or blending factor - -P[0] = mt.ones((M,M)) * 100 -xhat[0] = measurement[0] - -for k in range(1,N): - # time update - xhatminus[k] = A * xhat[k-1] - Pminus[k] = A * P[k-1] * A.T + Q - - # measurement update - K[k] = Pminus[k] * H.T * np.linalg.inv( H * Pminus[k] * H.T + R ) - xhat[k] = xhatminus[k] + K[k] * (measurement[k] - H * xhatminus[k]) - P[k] = ( I - K[k]*H ) * Pminus[k] - -t = np.arange(N) * dt -plt.figure() -plt.subplot(2,1,1) -plt.plot(t, measurement,'k+',label='noisy measurements') -plt.plot(t, xhat[:,0,0],'b-',label='position estimate') -plt.plot(t, x, 'r-', label='ground truth') - -plt.subplot(2,1,2) -plt.plot(t, xhat[:,1,0],'g-',label='velocity estimate') - -plt.show() diff --git a/filter-kalman/lang/nl_NL.ts b/filter-kalman/lang/nl_NL.ts deleted file mode 100644 index e3c61bbb..00000000 --- a/filter-kalman/lang/nl_NL.ts +++ /dev/null @@ -1,38 +0,0 @@ -<?xml version="1.0" encoding="utf-8"?> -<!DOCTYPE TS> -<TS version="2.1" language="nl_NL"> -<context> - <name>KalmanUICdialog_kalman</name> - <message> - <source>Kalman settings</source> - <translation type="unfinished"></translation> - </message> - <message> - <source>Measurement noise</source> - <translation type="unfinished"></translation> - </message> - <message> - <source>Rotation</source> - <translation type="unfinished"></translation> - </message> - <message> - <source>Position</source> - <translation type="unfinished"></translation> - </message> - <message> - <source>°</source> - <translation type="unfinished"></translation> - </message> - <message> - <source>-</source> - <translation type="unfinished"></translation> - </message> -</context> -<context> - <name>kalmanDll</name> - <message> - <source>Kalman</source> - <translation type="unfinished"></translation> - </message> -</context> -</TS> diff --git a/filter-kalman/lang/ru_RU.ts b/filter-kalman/lang/ru_RU.ts deleted file mode 100644 index 6fda97be..00000000 --- a/filter-kalman/lang/ru_RU.ts +++ /dev/null @@ -1,38 +0,0 @@ -<?xml version="1.0" encoding="utf-8"?> -<!DOCTYPE TS> -<TS version="2.1" language="ru_RU"> -<context> - <name>KalmanUICdialog_kalman</name> - <message> - <source>Kalman settings</source> - <translation type="unfinished"></translation> - </message> - <message> - <source>Measurement noise</source> - <translation type="unfinished"></translation> - </message> - <message> - <source>Rotation</source> - <translation type="unfinished"></translation> - </message> - <message> - <source>Position</source> - <translation type="unfinished"></translation> - </message> - <message> - <source>°</source> - <translation type="unfinished"></translation> - </message> - <message> - <source>-</source> - <translation type="unfinished"></translation> - </message> -</context> -<context> - <name>kalmanDll</name> - <message> - <source>Kalman</source> - <translation type="unfinished"></translation> - </message> -</context> -</TS> diff --git a/filter-kalman/lang/stub.ts b/filter-kalman/lang/stub.ts deleted file mode 100644 index 29fae5c0..00000000 --- a/filter-kalman/lang/stub.ts +++ /dev/null @@ -1,38 +0,0 @@ -<?xml version="1.0" encoding="utf-8"?> -<!DOCTYPE TS> -<TS version="2.1"> -<context> - <name>KalmanUICdialog_kalman</name> - <message> - <source>Kalman settings</source> - <translation type="unfinished"></translation> - </message> - <message> - <source>Measurement noise</source> - <translation type="unfinished"></translation> - </message> - <message> - <source>Rotation</source> - <translation type="unfinished"></translation> - </message> - <message> - <source>Position</source> - <translation type="unfinished"></translation> - </message> - <message> - <source>°</source> - <translation type="unfinished"></translation> - </message> - <message> - <source>-</source> - <translation type="unfinished"></translation> - </message> -</context> -<context> - <name>kalmanDll</name> - <message> - <source>Kalman</source> - <translation type="unfinished"></translation> - </message> -</context> -</TS> diff --git a/filter-kalman/lang/zh_CN.ts b/filter-kalman/lang/zh_CN.ts deleted file mode 100644 index d8de6cf2..00000000 --- a/filter-kalman/lang/zh_CN.ts +++ /dev/null @@ -1,38 +0,0 @@ -<?xml version="1.0" encoding="utf-8"?> -<!DOCTYPE TS> -<TS version="2.1" language="zh_CN"> -<context> - <name>KalmanUICdialog_kalman</name> - <message> - <source>Kalman settings</source> - <translation>Kalman 设置</translation> - </message> - <message> - <source>Measurement noise</source> - <translation type="unfinished">衡量噪声</translation> - </message> - <message> - <source>Rotation</source> - <translation>旋转</translation> - </message> - <message> - <source>Position</source> - <translation>方位</translation> - </message> - <message> - <source>°</source> - <translation></translation> - </message> - <message> - <source>-</source> - <translation type="unfinished"></translation> - </message> -</context> -<context> - <name>kalmanDll</name> - <message> - <source>Kalman</source> - <translation type="unfinished"></translation> - </message> -</context> -</TS> |