├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── Readme.md ├── include └── lightweight_filtering │ ├── CoordinateTransform.hpp │ ├── FilterBase.hpp │ ├── FilterState.hpp │ ├── GIFPrediction.hpp │ ├── ModelBase.hpp │ ├── OutlierDetection.hpp │ ├── Prediction.hpp │ ├── PropertyHandler.hpp │ ├── SigmaPoints.hpp │ ├── State.hpp │ ├── TestClasses.hpp │ ├── Update.hpp │ └── common.hpp ├── package.xml └── src ├── testFilterBase.cpp ├── testGIFPrediction.cpp ├── testModelBase.cpp ├── testPrediction.cpp ├── testSigmaPoints.cpp ├── testState.cpp └── testUpdate.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .cproject 2 | .project 3 | .settings 4 | build/ 5 | gtest/ 6 | cmake/ 7 | *~ 8 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.8) 2 | project(lightweight_filtering) 3 | add_definitions(-std=c++11) 4 | 5 | ##################### Find, include, and compile library ##################### 6 | 7 | # Attempt to find catkinized kindr 8 | find_package(kindr QUIET) 9 | if(NOT kindr_FOUND) 10 | # Attempt to find package-based kindr 11 | find_package(PkgConfig REQUIRED) 12 | pkg_check_modules(kindr kindr REQUIRED) 13 | endif() 14 | 15 | find_package(Eigen3 REQUIRED) 16 | 17 | include_directories(${kindr_INCLUDE_DIRS}) 18 | include_directories(${EIGEN3_INCLUDE_DIR}) 19 | include_directories(include) 20 | 21 | 22 | if(DEFINED CATKIN_DEVEL_PREFIX) 23 | find_package(catkin REQUIRED) 24 | catkin_package( 25 | INCLUDE_DIRS include ${kindr_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} 26 | ) 27 | endif() 28 | 29 | if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/gtest/" AND (NOT DEFINED CATKIN_DEVEL_PREFIX)) 30 | message(STATUS "Building GTests!") 31 | option(BUILD_GTEST "build gtest" ON) 32 | add_subdirectory(gtest gtest) 33 | enable_testing() 34 | include_directories(${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR}) 35 | add_executable(testState src/testState.cpp) 36 | target_link_libraries(testState gtest_main gtest pthread ) 37 | add_test(testState testState) 38 | add_executable(testSigmaPoints src/testSigmaPoints.cpp) 39 | target_link_libraries(testSigmaPoints gtest_main gtest pthread ) 40 | add_test(testSigmaPoints testSigmaPoints) 41 | add_executable(testPrediction src/testPrediction.cpp) 42 | target_link_libraries(testPrediction gtest_main gtest pthread ) 43 | add_test(testPrediction testPrediction) 44 | add_executable(testUpdate src/testUpdate.cpp) 45 | target_link_libraries(testUpdate gtest_main gtest pthread ) 46 | add_test(testUpdate testUpdate) 47 | add_executable(testModelBase src/testModelBase.cpp) 48 | target_link_libraries(testModelBase gtest_main gtest pthread ) 49 | add_test(testModelBase testModelBase) 50 | add_executable(testFilterBase src/testFilterBase.cpp) 51 | target_link_libraries(testFilterBase gtest_main gtest pthread ) 52 | add_test(testFilterBase testFilterBase) 53 | add_executable(testGIFPrediction src/testGIFPrediction.cpp) 54 | target_link_libraries(testGIFPrediction gtest_main gtest pthread ) 55 | add_test(testGIFPrediction testGIFPrediction) 56 | endif() 57 | 58 | # Generate FindLWF.cmake file 59 | file(WRITE cmake/FindLWF.cmake 60 | "# This file was automatically generated during the installation of the lightweight_filtering library 61 | # and can be used through cmake to find the corresponding header files. A copy of this 62 | # file was created in ${CMAKE_ROOT}/Modules (depending on the CMAKE_ROOT variable). 63 | 64 | set(LWF_INCLUDE_DIRS 65 | ${CMAKE_INSTALL_PREFIX}/include/LWF/include 66 | ) 67 | set(LWF_FOUND TRUE) 68 | message(\"-- LWF found (include: ${CMAKE_INSTALL_PREFIX}/include/LWF/include)\") 69 | " 70 | ) 71 | 72 | # Setting for make install 73 | install(CODE "execute_process(COMMAND ${CMAKE_COMMAND} -E make_directory ${CMAKE_INSTALL_PREFIX}/include/LWF)") 74 | install(DIRECTORY include DESTINATION ${CMAKE_INSTALL_PREFIX}/include/LWF) 75 | install(FILES cmake/FindLWF.cmake DESTINATION ${CMAKE_ROOT}/Modules) 76 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, Autonomous Systems Lab 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright 7 | notice, this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 12 | names of its contributors may be used to endorse or promote products 13 | derived from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 19 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 21 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 22 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 24 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /Readme.md: -------------------------------------------------------------------------------- 1 | Lightweight Filtering 2 | ===================== 3 | 4 | This is the lightweight filtering library. It provides basic functionalities for implementing EKF and UKF filters. 5 | 6 | Author(s): Michael Bloesch -------------------------------------------------------------------------------- /include/lightweight_filtering/CoordinateTransform.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * CoordinateTransform.hpp 3 | * 4 | * Created on: Feb 9, 2014 5 | * Author: Bloeschm 6 | */ 7 | 8 | #ifndef LWF_CoordinateTransform_HPP_ 9 | #define LWF_CoordinateTransform_HPP_ 10 | 11 | #include "lightweight_filtering/common.hpp" 12 | #include "lightweight_filtering/ModelBase.hpp" 13 | 14 | namespace LWF{ 15 | 16 | template 17 | class CoordinateTransform: public ModelBase,Output,Input>{ 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | typedef ModelBase,Output,Input> mtModelBase; 21 | typedef typename mtModelBase::mtInputTuple mtInputTuple; 22 | typedef Input mtInput; 23 | typedef Output mtOutput; 24 | Eigen::MatrixXd J_; 25 | Eigen::MatrixXd inverseProblem_C_; 26 | typename mtInput::mtDifVec inputDiff_; 27 | typename Eigen::LDLT inputLdlt_; 28 | typename mtInput::mtDifVec correction_; 29 | typename mtInput::mtDifVec lastCorrection_; 30 | typename mtOutput::mtDifVec outputDiff_; 31 | typename Eigen::LDLT outputLdlt_; 32 | mtOutput output_; 33 | CoordinateTransform(): J_((int)(mtOutput::D_),(int)(mtInput::D_)),inverseProblem_C_((int)(mtOutput::D_),(int)(mtOutput::D_)){ 34 | }; 35 | virtual ~CoordinateTransform(){}; 36 | void eval_(mtOutput& x, const mtInputTuple& inputs, double dt) const{ 37 | evalTransform(x,std::get<0>(inputs)); 38 | } 39 | template::type* = nullptr> 40 | void jacInput_(Eigen::MatrixXd& F, const mtInputTuple& inputs, double dt) const{ 41 | jacTransform(F,std::get<0>(inputs)); 42 | } 43 | virtual void evalTransform(mtOutput& output, const mtInput& input) const = 0; 44 | virtual void jacTransform(Eigen::MatrixXd& F, const mtInput& input) const = 0; 45 | void transformState(const mtInput& input, mtOutput& output) const{ 46 | evalTransform(output, input); 47 | } 48 | void transformCovMat(const mtInput& input,const Eigen::MatrixXd& inputCov,Eigen::MatrixXd& outputCov){ 49 | jacTransform(J_,input); 50 | outputCov = J_*inputCov*J_.transpose(); 51 | postProcess(outputCov,input); 52 | } 53 | virtual void postProcess(Eigen::MatrixXd& cov,const mtInput& input){} 54 | bool solveInverseProblem(mtInput& input,const Eigen::MatrixXd& inputCov, const mtOutput& outputRef, const double tolerance = 1e-6, const int max_iter = 10){ 55 | const mtInput inputRef = input; 56 | int count = 0; 57 | while(count < max_iter){ 58 | jacTransform(J_,input); 59 | inputRef.boxMinus(input,inputDiff_); 60 | transformState(input,output_); 61 | outputRef.boxMinus(output_,outputDiff_); 62 | inverseProblem_C_ = J_*inputCov*J_.transpose(); 63 | correction_ = inputDiff_ + inputCov*J_.transpose()*inverseProblem_C_.inverse()*(outputDiff_-J_*inputDiff_); 64 | input.boxPlus(correction_,input); 65 | if(correction_.norm() < tolerance){ 66 | return true; 67 | } 68 | count++; 69 | } 70 | return false; 71 | } 72 | bool solveInverseProblemRelaxed(mtInput& input,const Eigen::MatrixXd& inputCov, const mtOutput& outputRef,const Eigen::MatrixXd& outputCov, const double tolerance = 1e-6, const int max_iter = 10){ 73 | const mtInput inputRef = input; // TODO: correct all for boxminus Jacobian 74 | int count = 0; 75 | double startError; 76 | lastCorrection_.setZero(); 77 | while(count < max_iter){ 78 | jacTransform(J_,input); 79 | inputRef.boxMinus(input,inputDiff_); 80 | transformState(input,output_); 81 | outputRef.boxMinus(output_,outputDiff_); 82 | if(count==0){ 83 | inputLdlt_.compute(inputCov); 84 | outputLdlt_.compute(outputCov); 85 | startError = outputDiff_.dot(outputLdlt_.solve(outputDiff_)) + inputDiff_.dot(inputLdlt_.solve(inputDiff_)); 86 | } 87 | inverseProblem_C_ = J_*inputCov*J_.transpose()+outputCov; 88 | outputLdlt_.compute(inverseProblem_C_); 89 | correction_ = inputCov*J_.transpose()*outputLdlt_.solve(outputDiff_-J_*inputDiff_); 90 | inputRef.boxPlus(correction_,input); 91 | if((lastCorrection_-correction_).norm() < tolerance){ 92 | inputRef.boxMinus(input,inputDiff_); 93 | transformState(input,output_); 94 | outputRef.boxMinus(output_,outputDiff_); 95 | inputLdlt_.compute(inputCov); 96 | outputLdlt_.compute(outputCov); 97 | const double endError = outputDiff_.dot(outputLdlt_.solve(outputDiff_)) + inputDiff_.dot(inputLdlt_.solve(inputDiff_)); 98 | if(startError > endError){ 99 | return true; 100 | } else { 101 | return false; 102 | } 103 | } 104 | lastCorrection_ = correction_; 105 | count++; 106 | } 107 | return false; 108 | } 109 | bool testTransformJac(double d = 1e-6,double th = 1e-6){ 110 | mtInputTuple inputs; 111 | unsigned int s = 1; 112 | const double dt = 0.1; 113 | this->setRandomInputs(inputs,s); 114 | return this->testJacs(inputs,d,th,dt); 115 | } 116 | bool testTransformJac(const mtInput& input, double d = 1e-6,double th = 1e-6){ 117 | const double dt = 0.1; 118 | return this->testJacs(std::forward_as_tuple(input),d,th,dt); 119 | } 120 | }; 121 | 122 | } 123 | 124 | #endif /* LWF_CoordinateTransform_HPP_ */ 125 | -------------------------------------------------------------------------------- /include/lightweight_filtering/FilterBase.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FilterBase.hpp 3 | * 4 | * Created on: Feb 9, 2014 5 | * Author: Bloeschm 6 | */ 7 | 8 | #ifndef LWF_FilterBase_HPP_ 9 | #define LWF_FilterBase_HPP_ 10 | 11 | #include "lightweight_filtering/common.hpp" 12 | #include "lightweight_filtering/PropertyHandler.hpp" 13 | 14 | namespace LWF{ 15 | 16 | template 17 | class MeasurementTimeline{ 18 | public: 19 | typedef Meas mtMeas; 20 | std::map measMap_; 21 | typename std::map::iterator itMeas_; 22 | double maxWaitTime_; 23 | double minWaitTime_; 24 | MeasurementTimeline(){ 25 | maxWaitTime_ = 0.1; 26 | minWaitTime_ = 0.0; 27 | }; 28 | virtual ~MeasurementTimeline(){}; 29 | void addMeas(const mtMeas& meas, const double& t){ 30 | measMap_[t] = meas; 31 | } 32 | void clear() 33 | { 34 | measMap_.clear(); 35 | } 36 | void clean(double t){ 37 | while(measMap_.size() > 1 && measMap_.begin()->first<=t){ 38 | measMap_.erase(measMap_.begin()); 39 | } 40 | } 41 | bool getNextTime(double actualTime, double& nextTime){ 42 | itMeas_ = measMap_.upper_bound(actualTime); 43 | if(itMeas_!=measMap_.end()){ 44 | nextTime = itMeas_->first; 45 | return true; 46 | } else { 47 | return false; 48 | } 49 | } 50 | void waitTime(double actualTime, double& time){ 51 | double measurementTime = actualTime-maxWaitTime_; 52 | if(!measMap_.empty() && measMap_.rbegin()->first + minWaitTime_ > measurementTime){ 53 | measurementTime = measMap_.rbegin()->first + minWaitTime_; 54 | } 55 | if(time > measurementTime){ 56 | time = measurementTime; 57 | } 58 | } 59 | bool getLastTime(double& lastTime){ 60 | if(!measMap_.empty()){ 61 | lastTime = measMap_.rbegin()->first; 62 | return true; 63 | } else { 64 | return false; 65 | } 66 | } 67 | bool hasMeasurementAt(double t){ 68 | return measMap_.count(t)>0; 69 | } 70 | }; 71 | 72 | template 73 | class FilterBase: public PropertyHandler{ 74 | public: 75 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 76 | typedef Prediction mtPrediction; 77 | typedef typename mtPrediction::mtState mtState; 78 | static const unsigned int D_ = mtState::D_; 79 | static const int nUpdates_ = sizeof...(Updates); 80 | typedef typename mtPrediction::mtFilterState mtFilterState; 81 | mtFilterState safe_; 82 | mtFilterState front_; 83 | mtFilterState init_; 84 | MeasurementTimeline predictionTimeline_; 85 | std::tuple...> updateTimelineTuple_; 86 | mtPrediction mPrediction_; 87 | typedef std::tuple mtUpdates; 88 | mtUpdates mUpdates_; 89 | double safeWarningTime_; 90 | double frontWarningTime_; 91 | bool gotFrontWarning_; 92 | bool updateToUpdateMeasOnly_; 93 | unsigned int logCountMerPre_; 94 | unsigned int logCountRegPre_; 95 | unsigned int logCountBadPre_; 96 | unsigned int logCountComUpd_; 97 | unsigned int logCountRegUpd_; 98 | bool logCountDiagnostics_; 99 | FilterBase(){ 100 | init_.state_.setIdentity(); 101 | init_.cov_.setIdentity(); 102 | mPrediction_.doubleRegister_.registerScalar("alpha",init_.alpha_); 103 | mPrediction_.doubleRegister_.registerScalar("beta",init_.beta_); 104 | mPrediction_.doubleRegister_.registerScalar("kappa",init_.kappa_); 105 | init_.state_.registerElementsToPropertyHandler(this,"Init.State."); 106 | init_.state_.registerCovarianceToPropertyHandler_(init_.cov_,this,"Init.Covariance."); 107 | registerSubHandler("Prediction",mPrediction_); 108 | registerUpdates(); 109 | reset(); 110 | logCountDiagnostics_ = false; 111 | updateToUpdateMeasOnly_ = false; 112 | }; 113 | virtual ~FilterBase(){ 114 | }; 115 | void reset(double t = 0.0){ 116 | init_.t_ = t; 117 | init_.state_.fix(); 118 | safe_ = init_; 119 | front_ = init_; 120 | safeWarningTime_ = t; 121 | frontWarningTime_ = t; 122 | gotFrontWarning_ = false; 123 | } 124 | template::type* = nullptr> 125 | void registerUpdates(){ 126 | registerSubHandler("Update" + std::to_string(i),std::get(mUpdates_)); 127 | std::get(mUpdates_).outlierDetection_.registerToPropertyHandler(&std::get(mUpdates_),"MahalanobisTh"); 128 | registerUpdates(); 129 | } 130 | template=nUpdates_)>::type* = nullptr> 131 | void registerUpdates(){ 132 | } 133 | void addPredictionMeas(const typename Prediction::mtMeas& meas, double t){ 134 | if(t<= safeWarningTime_) { 135 | std::cout << "[FilterBase::addPredictionMeas] Warning: included measurements at time " << t << " before safeTime " << safeWarningTime_ << std::endl; 136 | } 137 | 138 | if(t<= frontWarningTime_) gotFrontWarning_ = true; 139 | predictionTimeline_.addMeas(meas,t); 140 | } 141 | template 142 | void addUpdateMeas(const typename std::tuple_element::type::mtMeas& meas, double t){ 143 | if(t<= safeWarningTime_) { 144 | std::cout << "[FilterBase::addUpdateMeas] Warning: included measurements at time " << t << " before safeTime " << safeWarningTime_ << std::endl; 145 | } 146 | if(t<= frontWarningTime_) gotFrontWarning_ = true; 147 | std::get(updateTimelineTuple_).addMeas(meas,t); 148 | } 149 | bool getSafeTime(double& safeTime){ 150 | double maxPredictionTime; 151 | if(!predictionTimeline_.getLastTime(maxPredictionTime)){ 152 | safeTime = safe_.t_; 153 | return false; 154 | } 155 | safeTime = maxPredictionTime; 156 | // Check if we have to wait for update measurements 157 | checkUpdateWaitTime(maxPredictionTime,safeTime); 158 | if(safeTime <= safe_.t_){ 159 | safeTime = safe_.t_; 160 | return false; 161 | } 162 | return true; 163 | } 164 | template::type* = nullptr> 165 | void checkUpdateWaitTime(double actualTime,double& time){ 166 | std::get(updateTimelineTuple_).waitTime(actualTime,time); 167 | checkUpdateWaitTime(actualTime,time); 168 | } 169 | template=nUpdates_)>::type* = nullptr> 170 | void checkUpdateWaitTime(double actualTime,double& time){ 171 | } 172 | void updateSafe(const double* maxTime = nullptr){ 173 | double nextSafeTime; 174 | bool gotSafeTime = getSafeTime(nextSafeTime); 175 | if(!gotSafeTime || (maxTime != nullptr && *maxTime < safe_.t_)){ 176 | if(logCountDiagnostics_){ 177 | std::cout << "Performed safe Update with RegPre: 0, MerPre: 0, BadPre: 0, RegUpd: 0, ComUpd: 0" << std::endl; 178 | } 179 | return; 180 | } 181 | if(maxTime != nullptr && nextSafeTime > *maxTime) nextSafeTime = *maxTime; 182 | if(front_.t_<=nextSafeTime && !gotFrontWarning_ && front_.t_>safe_.t_){ 183 | safe_ = front_; 184 | } 185 | update(safe_,nextSafeTime); 186 | clean(safe_.t_); 187 | safeWarningTime_ = safe_.t_; 188 | if(logCountDiagnostics_){ 189 | std::cout << "Performed safe Update with RegPre: " << logCountRegPre_ << ", MerPre: " << logCountMerPre_ << ", BadPre: " << logCountBadPre_ << ", RegUpd: " << logCountRegUpd_ << ", ComUpd: " << logCountComUpd_ << std::endl; 190 | } 191 | } 192 | void updateFront(const double& tEnd){ 193 | updateSafe(); 194 | if(gotFrontWarning_ || front_.t_<=safe_.t_){ 195 | front_ = safe_; 196 | } 197 | update(front_,tEnd); 198 | frontWarningTime_ = front_.t_; 199 | gotFrontWarning_ = false; 200 | } 201 | void update(mtFilterState& filterState,const double& tEnd){ 202 | double tNext = filterState.t_; 203 | logCountMerPre_ = 0; 204 | logCountRegPre_ = 0; 205 | logCountBadPre_ = 0; 206 | logCountComUpd_ = 0; 207 | logCountRegUpd_ = 0; 208 | while(filterState.t_second,std::min(predictionTimeline_.itMeas_->first,tNext)-filterState.t_); 221 | if(r!=0) std::cout << "Error during performPrediction: " << r << std::endl; 222 | logCountRegPre_++; 223 | } 224 | } 225 | if(filterState.t_ < tNext){ 226 | r = mPrediction_.performPrediction(filterState,tNext-filterState.t_); 227 | if(r!=0) std::cout << "Error during performPrediction: " << r << std::endl; 228 | logCountBadPre_++; 229 | } 230 | doAvailableUpdates(filterState,tNext); 231 | } 232 | } 233 | template::type* = nullptr> 234 | bool getNextUpdate(double actualTime, double& nextTime){ 235 | double tNextUpdate; 236 | bool gotMatchingUpdate = false; 237 | if(std::get(updateTimelineTuple_).getNextTime(actualTime,tNextUpdate) && tNextUpdate < nextTime){ 238 | gotMatchingUpdate = true; 239 | nextTime = tNextUpdate; 240 | } 241 | return gotMatchingUpdate | getNextUpdate(actualTime, nextTime); 242 | } 243 | template=nUpdates_)>::type* = nullptr> 244 | bool getNextUpdate(double actualTime, double& nextTime){ 245 | return false; 246 | } 247 | template::type* = nullptr> 248 | void doAvailableUpdates(mtFilterState& filterState, double tNext){ 249 | if(std::get(updateTimelineTuple_).hasMeasurementAt(tNext)){ 250 | int r = std::get(mUpdates_).performUpdate(filterState,std::get(updateTimelineTuple_).measMap_[tNext]); 251 | if(r!=0) std::cout << "Error during update: " << r << std::endl; 252 | logCountRegUpd_++; 253 | } 254 | doAvailableUpdates(filterState,tNext); 255 | } 256 | template=nUpdates_)>::type* = nullptr> 257 | void doAvailableUpdates(mtFilterState& filterState, double tNext){ 258 | } 259 | void clean(const double& t){ 260 | predictionTimeline_.clean(t); 261 | cleanUpdateTimeline(t); 262 | } 263 | template::type* = nullptr> 264 | void cleanUpdateTimeline(const double& t){ 265 | std::get(updateTimelineTuple_).clean(t); 266 | cleanUpdateTimeline(t); 267 | } 268 | template=nUpdates_)>::type* = nullptr> 269 | void cleanUpdateTimeline(const double& t){ 270 | } 271 | }; 272 | 273 | } 274 | 275 | #endif /* LWF_FilterBase_HPP_ */ 276 | -------------------------------------------------------------------------------- /include/lightweight_filtering/FilterState.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FilterState.hpp 3 | * 4 | * Created on: Feb 9, 2014 5 | * Author: Bloeschm 6 | */ 7 | 8 | #ifndef LWF_FILTERSTATE_HPP_ 9 | #define LWF_FILTERSTATE_HPP_ 10 | 11 | #include "lightweight_filtering/common.hpp" 12 | #include "lightweight_filtering/SigmaPoints.hpp" 13 | 14 | namespace LWF{ 15 | 16 | template 17 | class FilterState{ 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | typedef State mtState; 21 | typedef PredictionMeas mtPredictionMeas; 22 | typedef PredictionNoise mtPredictionNoise; 23 | FilteringMode mode_; 24 | bool usePredictionMerge_; 25 | static constexpr unsigned int noiseExtensionDim_ = noiseExtensionDim; 26 | double t_; 27 | mtState state_; 28 | Eigen::MatrixXd cov_; 29 | Eigen::MatrixXd F_; 30 | Eigen::MatrixXd G_; 31 | SigmaPoints stateSigmaPoints_; 32 | SigmaPoints stateSigmaPointsNoi_; 33 | SigmaPoints stateSigmaPointsPre_; 34 | Eigen::MatrixXd prenoiP_; // automatic change tracking 35 | typename mtState::mtDifVec difVecLin_; 36 | double alpha_; 37 | double beta_; 38 | double kappa_; 39 | FilterState(): cov_((int)(mtState::D_),(int)(mtState::D_)), 40 | F_((int)(mtState::D_),(int)(mtState::D_)), 41 | G_((int)(mtState::D_),(int)(mtPredictionNoise::D_)), 42 | prenoiP_((int)(mtPredictionNoise::D_),(int)(mtPredictionNoise::D_)){ 43 | alpha_ = 1e-3; 44 | beta_ = 2.0; 45 | kappa_ = 0.0; 46 | mode_ = ModeEKF; 47 | usePredictionMerge_ = false; 48 | t_ = 0.0; 49 | state_.setIdentity(); 50 | cov_.setIdentity(); 51 | F_.setIdentity(); 52 | G_.setZero(); 53 | prenoiP_.setIdentity(); 54 | prenoiP_ *= 0.0001; 55 | difVecLin_.setIdentity(); 56 | refreshUKFParameter(); 57 | } 58 | virtual ~FilterState(){}; 59 | void refreshUKFParameter(){ 60 | stateSigmaPoints_.computeParameter(alpha_,beta_,kappa_); 61 | stateSigmaPointsNoi_.computeParameter(alpha_,beta_,kappa_); 62 | stateSigmaPointsPre_.computeParameter(alpha_,beta_,kappa_); 63 | stateSigmaPointsNoi_.computeFromZeroMeanGaussian(prenoiP_); 64 | } 65 | void refreshNoiseSigmaPoints(const Eigen::MatrixXd& prenoiP){ 66 | if(prenoiP_ != prenoiP){ 67 | prenoiP_ = prenoiP; 68 | stateSigmaPointsNoi_.computeFromZeroMeanGaussian(prenoiP_); 69 | } 70 | } 71 | }; 72 | 73 | } 74 | 75 | #endif /* LWF_FILTERSTATE_HPP_ */ 76 | -------------------------------------------------------------------------------- /include/lightweight_filtering/GIFPrediction.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GIFPrediction.hpp 3 | * 4 | * Created on: Aug 20, 2015 5 | * Author: Bloeschm 6 | */ 7 | 8 | #ifndef LWF_GIFPREDICTIONMODEL_HPP_ 9 | #define LWF_GIFPREDICTIONMODEL_HPP_ 10 | 11 | #include "lightweight_filtering/common.hpp" 12 | #include "lightweight_filtering/ModelBase.hpp" 13 | #include "lightweight_filtering/PropertyHandler.hpp" 14 | 15 | namespace LWF{ 16 | 17 | template 18 | class GIFPrediction: public ModelBase,Innovation,typename FilterState::mtState,typename FilterState::mtState,Noise>, public PropertyHandler{ 19 | public: 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | typedef ModelBase,Innovation,typename FilterState::mtState,typename FilterState::mtState,Noise> mtModelBase; 22 | typedef FilterState mtFilterState; 23 | typedef Innovation mtInnovation; 24 | typedef typename mtModelBase::mtInputTuple mtInputTuple; 25 | typedef typename mtFilterState::mtState mtState; 26 | typedef Meas mtMeas; 27 | typedef Noise mtNoise; 28 | Eigen::MatrixXd noiP_; 29 | Eigen::MatrixXd noiPwgt_; 30 | Eigen::MatrixXd noiPinv_; 31 | bool disablePreAndPostProcessingWarning_; 32 | mtState stateCurrentLin_; 33 | Eigen::MatrixXd jacPreviousState_; 34 | Eigen::MatrixXd jacCurrentState_; 35 | Eigen::MatrixXd jacNoise_; 36 | mtInnovation r_; 37 | typename mtInnovation::mtDifVec dr_; 38 | typename mtState::mtDifVec dx_; 39 | mtMeas meas_; 40 | Eigen::MatrixXd A00_; 41 | Eigen::MatrixXd A01_; 42 | Eigen::MatrixXd A11_; 43 | Eigen::MatrixXd S_; 44 | Eigen::MatrixXd Sinv_; 45 | GIFPrediction(): noiP_((int)(mtNoise::D_),(int)(mtNoise::D_)), 46 | noiPinv_((int)(mtInnovation::D_),(int)(mtInnovation::D_)), 47 | jacPreviousState_((int)(mtInnovation::D_),(int)(mtState::D_)), 48 | jacCurrentState_((int)(mtInnovation::D_),(int)(mtState::D_)), 49 | jacNoise_((int)(mtInnovation::D_),(int)(mtNoise::D_)), 50 | A00_((int)(mtState::D_),(int)(mtState::D_)), 51 | A01_((int)(mtState::D_),(int)(mtState::D_)), 52 | A11_((int)(mtState::D_),(int)(mtState::D_)), 53 | S_((int)(mtState::D_),(int)(mtState::D_)), 54 | Sinv_((int)(mtState::D_),(int)(mtState::D_)){ 55 | noiP_.setIdentity(); 56 | noiP_ *= 0.0001; 57 | mtNoise n; 58 | n.setIdentity(); 59 | n.registerCovarianceToPropertyHandler_(noiP_,this,"Noise."); 60 | disablePreAndPostProcessingWarning_ = false; 61 | refreshProperties(); 62 | }; 63 | virtual ~GIFPrediction(){}; 64 | void refreshProperties(){ 65 | noiPinv_.setIdentity(); 66 | // noiP_.llt().solveInPlace(noiPinv_); // TODO: fix and improve 67 | } 68 | void eval_(mtInnovation& y, const mtInputTuple& inputs, double dt) const{ 69 | evalResidual(y,std::get<0>(inputs),std::get<1>(inputs),std::get<2>(inputs),dt); 70 | } 71 | template::type* = nullptr> 72 | void jacInput_(Eigen::MatrixXd& F, const mtInputTuple& inputs, double dt) const{ 73 | jacPreviousState(F,std::get<0>(inputs),std::get<1>(inputs),dt); 74 | } 75 | template::type* = nullptr> 76 | void jacInput_(Eigen::MatrixXd& F, const mtInputTuple& inputs, double dt) const{ 77 | jacCurrentState(F,std::get<0>(inputs),std::get<1>(inputs),dt); 78 | } 79 | template::type* = nullptr> 80 | void jacInput_(Eigen::MatrixXd& F, const mtInputTuple& inputs, double dt) const{ 81 | jacNoise(F,std::get<0>(inputs),std::get<1>(inputs),dt); 82 | } 83 | virtual void evalResidual(mtInnovation& y, const mtState& previousState, const mtState& currentState, const mtNoise& noise, double dt) const = 0; 84 | virtual void evalResidualShort(mtInnovation& y, const mtState& previousState, const mtState& currentState, double dt) const{ 85 | mtNoise n; // TODO get static for Identity() 86 | n.setIdentity(); 87 | evalResidual(y,previousState,currentState,n,dt); 88 | } 89 | virtual void jacPreviousState(Eigen::MatrixXd& F, const mtState& previousState, const mtState& currentState, double dt) const = 0; 90 | virtual void jacCurrentState(Eigen::MatrixXd& F, const mtState& previousState, const mtState& currentState, double dt) const = 0; 91 | virtual void jacNoise(Eigen::MatrixXd& F, const mtState& previousState, const mtState& currentState, double dt) const = 0; 92 | virtual void noMeasCase(mtFilterState& filterState, mtMeas& meas, double dt){}; 93 | virtual void preProcess(mtFilterState& filterState, const mtMeas& meas, double dt){ 94 | if(!disablePreAndPostProcessingWarning_){ 95 | std::cout << "Warning: preProcessing is not implement!" << std::endl; 96 | } 97 | }; 98 | virtual void postProcess(mtFilterState& filterState, const mtMeas& meas, double dt){ 99 | if(!disablePreAndPostProcessingWarning_){ 100 | std::cout << "Warning: postProcessing is not implement!" << std::endl; 101 | } 102 | }; 103 | int performPrediction(mtFilterState& filterState, double dt){ 104 | mtMeas meas; 105 | meas.setIdentity(); 106 | noMeasCase(filterState,meas,dt); 107 | return performPrediction(filterState,meas,dt); 108 | } 109 | int performPrediction(mtFilterState& filterState, const mtMeas& meas, double dt){ 110 | meas_ = meas; 111 | preProcess(filterState,meas,dt); 112 | getLinearizationPoint(stateCurrentLin_,filterState,meas,dt); 113 | jacPreviousState(jacPreviousState_,filterState.state_,stateCurrentLin_,dt); 114 | if(!jacPreviousState_.allFinite()) std::cout << "jacPreviousState_ is BAD" << std::endl; 115 | jacCurrentState(jacCurrentState_,filterState.state_,stateCurrentLin_,dt); 116 | if(!jacCurrentState_.allFinite()) std::cout << "jacCurrentState_ is BAD" << std::endl; 117 | jacNoise(jacNoise_,filterState.state_,stateCurrentLin_,dt); 118 | if(!jacNoise_.allFinite()) std::cout << "jacNoise_ is BAD" << std::endl; 119 | this->evalResidualShort(r_,filterState.state_,stateCurrentLin_,dt); 120 | r_.boxMinus(mtInnovation::Identity(),dr_); 121 | if(!dr_.allFinite()) std::cout << "dr_ is BAD" << std::endl; 122 | noiPinv_.setIdentity(); 123 | (jacNoise_*noiP_*jacNoise_.transpose()).llt().solveInPlace(noiPinv_); // Make more efficient 124 | if(!noiPinv_.allFinite()) std::cout << "noiPinv_ is BAD" << std::endl; 125 | A00_ = jacPreviousState_.transpose()*noiPinv_*jacPreviousState_; 126 | A01_ = jacPreviousState_.transpose()*noiPinv_*jacCurrentState_; 127 | A11_ = jacCurrentState_.transpose()*noiPinv_*jacCurrentState_; 128 | S_ = filterState.cov_ + A00_; 129 | Sinv_.setIdentity(); 130 | S_.ldlt().solveInPlace(Sinv_); // TODO: check accuracy 131 | if(!Sinv_.allFinite()) std::cout << "Sinv_ is BAD" << std::endl; 132 | filterState.cov_ = A11_ - A01_.transpose()*Sinv_*A01_; 133 | if(!filterState.cov_.allFinite()) std::cout << "filterState.cov_ is BAD" << std::endl; 134 | dx_ = - jacCurrentState_.transpose()*noiPinv_*dr_ + A01_.transpose()*Sinv_*jacPreviousState_.transpose()*noiPinv_*dr_; 135 | if(!dx_.allFinite()) std::cout << "dx_ is BAD before inverse" << std::endl; 136 | enforceSymmetry(filterState.cov_); 137 | filterState.cov_.ldlt().solveInPlace(dx_); // TODO: check accuracy 138 | if(!dx_.allFinite()) std::cout << "dx_ is BAD after inverse" << std::endl; 139 | stateCurrentLin_.boxPlus(dx_,filterState.state_); 140 | filterState.state_.fix(); 141 | filterState.t_ += dt; 142 | postProcess(filterState,meas,dt); 143 | return 0; 144 | } 145 | virtual void getLinearizationPoint(mtState& state1, const mtFilterState& filterState, const mtMeas& meas, double dt){ 146 | state1 = filterState.state_; 147 | }; 148 | int predictMerged(mtFilterState& filterState, double tTarget, const std::map& measMap){ 149 | std::cout << "\033[31mGIF predictions cannot be merged!\033[0m" << std::endl; 150 | return 1; 151 | } 152 | bool testPredictionJacs(double d = 1e-6,double th = 1e-6,double dt = 0.1){ 153 | mtState previousState; 154 | mtState currentState; 155 | mtMeas meas; 156 | unsigned int s = 1; 157 | previousState.setRandom(s); 158 | currentState.setRandom(s); 159 | meas.setRandom(s); 160 | return testPredictionJacs(previousState,currentState,meas,d,th,dt); 161 | } 162 | bool testPredictionJacs(const mtState& previousState,const mtState& currentState, const mtMeas& meas, double d = 1e-6,double th = 1e-6,double dt = 0.1){ 163 | mtInputTuple inputs; 164 | std::get<0>(inputs) = previousState; 165 | std::get<1>(inputs) = currentState; 166 | std::get<2>(inputs).setIdentity(); // Noise is always set to zero for Jacobians 167 | meas_ = meas; 168 | return this->testJacs(inputs,d,th,dt); 169 | } 170 | }; 171 | 172 | } 173 | 174 | #endif /* LWF_GIFPREDICTIONMODEL_HPP_ */ 175 | -------------------------------------------------------------------------------- /include/lightweight_filtering/ModelBase.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ModelBase.hpp 3 | * 4 | * Created on: Feb 9, 2014 5 | * Author: Bloeschm 6 | */ 7 | 8 | #ifndef LWF_ModelBase_HPP_ 9 | #define LWF_ModelBase_HPP_ 10 | 11 | #include "lightweight_filtering/common.hpp" 12 | 13 | namespace LWF{ 14 | 15 | template 16 | class ModelBase{ 17 | public: 18 | static const unsigned int nInputs_ = sizeof...(Inputs); 19 | typedef Output mtOutput; 20 | typedef std::tuple mtInputTuple; 21 | ModelBase(){}; 22 | virtual ~ModelBase(){}; 23 | void eval(mtOutput& output, const mtInputTuple& inputs, double dt = 0.0) const{ 24 | static_cast(*this).eval_(output,inputs,dt); 25 | } 26 | template 27 | void jacInput(Eigen::MatrixXd& F, const mtInputTuple& inputs, double dt = 0.0) const{ 28 | static_cast(*this).template jacInput_(F,inputs,dt); 29 | } 30 | template::type::D_> 31 | void jacInputFD(Eigen::MatrixXd& F, const mtInputTuple& inputs, double dt, double d) const{ 32 | static_assert(s + n <= (std::tuple_element::type::D_), "Bad dimension for evaluating jacInputFD"); 33 | mtInputTuple inputsDisturbed = inputs; 34 | typename std::tuple_element::type::mtDifVec difVec; 35 | mtOutput outputReference; 36 | mtOutput outputDisturbed; 37 | eval(outputReference,inputs,dt); 38 | typename mtOutput::mtDifVec dif; 39 | for(unsigned int j=s;j(inputs).boxPlus(difVec,std::get(inputsDisturbed)); 43 | eval(outputDisturbed,inputsDisturbed,dt); 44 | outputDisturbed.boxMinus(outputReference,dif); 45 | F.col(j) = dif/d; 46 | } 47 | } 48 | template 49 | bool testJacInput(double d = 1e-6,double th = 1e-6,unsigned int s = 0,double dt = 0.1) const{ 50 | mtInputTuple inputs; 51 | setRandomInputs(inputs,s); 52 | return testJacInput(inputs,d,th,dt); 53 | } 54 | template 55 | bool testJacInput(const mtInputTuple& inputs, double d = 1e-6,double th = 1e-6,double dt = 0.1) const{ 56 | Eigen::MatrixXd F((int)(mtOutput::D_),(int)(std::tuple_element::type::D_)); 57 | Eigen::MatrixXd F_FD((int)(mtOutput::D_),(int)(std::tuple_element::type::D_)); 58 | mtOutput output; 59 | typename Eigen::MatrixXd::Index maxRow, maxCol = 0; 60 | jacInput(F,inputs,dt); 61 | jacInputFD(F_FD,inputs,dt,d); 62 | const double r = (F-F_FD).array().abs().maxCoeff(&maxRow, &maxCol); 63 | if(r>th){ 64 | unsigned int outputId = mtOutput::getElementId(maxRow); 65 | unsigned int inputId = std::tuple_element::type::getElementId(maxCol); 66 | std::cout << "==== Model jacInput Test failed: " << r << " is larger than " << th << " at row " 67 | << maxRow << "("<< output.getName(outputId) << ") and col " << maxCol << "("<< std::get(inputs).getName(inputId) << ") ====" << std::endl; 68 | std::cout << " " << F(maxRow,maxCol) << " " << F_FD(maxRow,maxCol) << std::endl; 69 | return false; 70 | } else { 71 | std::cout << "==== Test successful (" << r << ") ====" << std::endl; 72 | return true; 73 | } 74 | } 75 | static inline void setRandomInputs(mtInputTuple& inputs,unsigned int& s){ 76 | _setRandomInputs(inputs,s); 77 | } 78 | template::type* = nullptr> 79 | static inline void _setRandomInputs(mtInputTuple& inputs,unsigned int& s){ 80 | std::get(inputs).setRandom(s); 81 | _setRandomInputs(inputs,s); 82 | } 83 | template=nInputs_)>::type* = nullptr> 84 | static inline void _setRandomInputs(mtInputTuple& inputs,unsigned int& s){} 85 | bool testJacs(unsigned int& s, double d = 1e-6,double th = 1e-6,double dt = 0.1) const{ 86 | mtInputTuple inputs; 87 | setRandomInputs(inputs,s); 88 | return testJacs(inputs,d,th,dt); 89 | } 90 | bool testJacs(double d = 1e-6,double th = 1e-6,double dt = 0.1) const{ 91 | mtInputTuple inputs; 92 | unsigned int s = 1; 93 | setRandomInputs(inputs,s); 94 | return testJacs(inputs,d,th,dt); 95 | } 96 | bool testJacs(const mtInputTuple& inputs, double d = 1e-6,double th = 1e-6,double dt = 0.1) const{ 97 | return _testJacs(inputs,d,th,dt); 98 | } 99 | template::type* = nullptr> 100 | bool _testJacs(const mtInputTuple& inputs, double d = 1e-6,double th = 1e-6,double dt = 0.1) const{ 101 | return (testJacInput(inputs,d,th,dt) & _testJacs(inputs,d,th,dt)); 102 | } 103 | template=nInputs_)>::type* = nullptr> 104 | bool _testJacs(const mtInputTuple& inputs, double d = 1e-6,double th = 1e-6,double dt = 0.1) const{ 105 | return true; 106 | } 107 | }; 108 | 109 | } 110 | 111 | #endif /* LWF_ModelBase_HPP_ */ 112 | -------------------------------------------------------------------------------- /include/lightweight_filtering/OutlierDetection.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OutlierDetection.hpp 3 | * 4 | * Created on: Feb 9, 2014 5 | * Author: Bloeschm 6 | */ 7 | 8 | #ifndef LWF_OUTLIERDETECTION_HPP_ 9 | #define LWF_OUTLIERDETECTION_HPP_ 10 | 11 | #include "lightweight_filtering/common.hpp" 12 | #include "lightweight_filtering/PropertyHandler.hpp" 13 | 14 | namespace LWF{ 15 | 16 | template struct ODEntry{ 17 | static const unsigned int S_ = S; 18 | static const unsigned int D_ = D; 19 | }; 20 | 21 | template 22 | class OutlierDetectionBase{ 23 | public: 24 | static const unsigned int S_ = S; 25 | static const unsigned int D_ = D; 26 | bool outlier_; 27 | bool enabled_; 28 | double mahalanobisTh_; 29 | double d_; 30 | unsigned int outlierCount_; 31 | OutlierDetectionBase(){ 32 | mahalanobisTh_ = -0.0376136*D_*D_+1.99223*D_+2.05183; // Quadratic fit to chi square 33 | enabled_ = false; 34 | outlier_ = false; 35 | outlierCount_ = 0; 36 | d_ = 0; 37 | } 38 | virtual ~OutlierDetectionBase(){}; 39 | template 40 | void check(const Eigen::Matrix& innVector,const Eigen::MatrixXd& Py){ 41 | d_ = ((innVector.template block(S_,0)).transpose()*Py.template block(S_,S_).inverse()*innVector.template block(S_,0))(0,0); 42 | outlier_ = d_ > mahalanobisTh_; 43 | if(outlier_){ 44 | outlierCount_++; 45 | } else { 46 | outlierCount_ = 0; 47 | } 48 | } 49 | virtual void registerToPropertyHandler(PropertyHandler* mpPropertyHandler, const std::string& str, unsigned int i = 0) = 0; 50 | virtual void reset() = 0; 51 | virtual bool isOutlier(unsigned int i) const = 0; 52 | virtual void setEnabled(unsigned int i,bool enabled) = 0; 53 | virtual void setEnabledAll(bool enabled) = 0; 54 | virtual unsigned int& getCount(unsigned int i) = 0; 55 | virtual double& getMahalTh(unsigned int i) = 0; 56 | virtual double getMahalDistance(unsigned int i) const = 0; 57 | }; 58 | 59 | template 60 | class OutlierDetectionConcat: public OutlierDetectionBase{ 61 | public: 62 | using OutlierDetectionBase::S_; 63 | using OutlierDetectionBase::D_; 64 | using OutlierDetectionBase::outlier_; 65 | using OutlierDetectionBase::enabled_; 66 | using OutlierDetectionBase::mahalanobisTh_; 67 | using OutlierDetectionBase::outlierCount_; 68 | using OutlierDetectionBase::check; 69 | using OutlierDetectionBase::d_; 70 | T sub_; 71 | OutlierDetectionConcat(){}; 72 | virtual ~OutlierDetectionConcat(){}; 73 | template 74 | void doOutlierDetection(const Eigen::Matrix& innVector,Eigen::MatrixXd& Py,Eigen::MatrixXd& H){ 75 | static_assert(dI>=S+D,"Outlier detection out of range"); 76 | check(innVector,Py); 77 | outlier_ = outlier_ & enabled_; 78 | sub_.doOutlierDetection(innVector,Py,H); 79 | if(outlier_){ 80 | Py.block(0,S_,dI,D_).setZero(); 81 | Py.block(S_,0,D_,dI).setZero(); 82 | Py.block(S_,S_,D_,D_).setIdentity(); 83 | H.block(S_,0,D_,H.cols()).setZero(); 84 | } 85 | } 86 | void registerToPropertyHandler(PropertyHandler* mpPropertyHandler, const std::string& str, unsigned int i = 0){ 87 | mpPropertyHandler->doubleRegister_.registerScalar(str + std::to_string(i), mahalanobisTh_); 88 | sub_.registerToPropertyHandler(mpPropertyHandler,str,i+1); 89 | } 90 | void reset(){ 91 | outlier_ = false; 92 | outlierCount_ = 0; 93 | sub_.reset(); 94 | } 95 | bool isOutlier(unsigned int i) const{ 96 | if(i==0){ 97 | return outlier_; 98 | } else { 99 | return sub_.isOutlier(i-1); 100 | } 101 | } 102 | void setEnabled(unsigned int i,bool enabled){ 103 | if(i==0){ 104 | enabled_ = enabled; 105 | } else { 106 | sub_.setEnabled(i-1,enabled); 107 | } 108 | } 109 | void setEnabledAll(bool enabled){ 110 | enabled_ = enabled; 111 | sub_.setEnabledAll(enabled); 112 | } 113 | unsigned int& getCount(unsigned int i){ 114 | if(i==0){ 115 | return outlierCount_; 116 | } else { 117 | return sub_.getCount(i-1); 118 | } 119 | } 120 | double& getMahalTh(unsigned int i){ 121 | if(i==0){ 122 | return mahalanobisTh_; 123 | } else { 124 | return sub_.getMahalTh(i-1); 125 | } 126 | } 127 | double getMahalDistance(unsigned int i) const{ 128 | if(i==0){ 129 | return d_; 130 | } else { 131 | return sub_.getMahalDistance(i-1); 132 | } 133 | } 134 | }; 135 | 136 | class OutlierDetectionDefault: public OutlierDetectionBase<0,0>{ 137 | public: 138 | using OutlierDetectionBase<0,0>::mahalanobisTh_; 139 | using OutlierDetectionBase<0,0>::outlierCount_; 140 | OutlierDetectionDefault(){}; 141 | virtual ~OutlierDetectionDefault(){}; 142 | template 143 | void doOutlierDetection(const Eigen::Matrix& innVector,Eigen::MatrixXd& Py,Eigen::MatrixXd& H){ 144 | } 145 | void registerToPropertyHandler(PropertyHandler* mpPropertyHandler, const std::string& str, unsigned int i = 0){ 146 | } 147 | void reset(){ 148 | } 149 | bool isOutlier(unsigned int i) const{ 150 | throw std::runtime_error("Outlier index out of range."); 151 | return false; 152 | } 153 | void setEnabled(unsigned int i,bool enabled){ 154 | throw std::runtime_error("Outlier index out of range."); 155 | } 156 | void setEnabledAll(bool enabled){ 157 | } 158 | unsigned int& getCount(unsigned int i){ 159 | throw std::runtime_error("Outlier index out of range."); 160 | return outlierCount_; 161 | } 162 | double& getMahalTh(unsigned int i){ 163 | throw std::runtime_error("Outlier index out of range."); 164 | return mahalanobisTh_; 165 | } 166 | double getMahalDistance(unsigned int i) const{ 167 | throw std::runtime_error("Outlier index out of range."); 168 | return d_; 169 | } 170 | }; 171 | 172 | template 173 | class OutlierDetection{ 174 | public: 175 | virtual ~OutlierDetection(){}; 176 | }; 177 | template 178 | class OutlierDetection,ODEntries...>: public OutlierDetectionConcat,ODEntries...>>{ 179 | public: 180 | virtual ~OutlierDetection(){}; 181 | }; 182 | template 183 | class OutlierDetection,ODEntries...>: public OutlierDetectionConcat>{ 184 | public: 185 | virtual ~OutlierDetection(){}; 186 | }; 187 | template 188 | class OutlierDetection,ODEntries...>: public OutlierDetection{ 189 | public: 190 | virtual ~OutlierDetection(){}; 191 | }; 192 | template 193 | class OutlierDetection>: public OutlierDetectionConcat>>{ 194 | public: 195 | virtual ~OutlierDetection(){}; 196 | }; 197 | template 198 | class OutlierDetection>: public OutlierDetectionConcat{ 199 | public: 200 | virtual ~OutlierDetection(){}; 201 | }; 202 | template 203 | class OutlierDetection>: public OutlierDetectionDefault{ 204 | public: 205 | virtual ~OutlierDetection(){}; 206 | }; 207 | 208 | } 209 | 210 | #endif /* LWF_OUTLIERDETECTION_HPP_ */ 211 | -------------------------------------------------------------------------------- /include/lightweight_filtering/Prediction.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Prediction.hpp 3 | * 4 | * Created on: Feb 9, 2014 5 | * Author: Bloeschm 6 | */ 7 | 8 | #ifndef LWF_PREDICTIONMODEL_HPP_ 9 | #define LWF_PREDICTIONMODEL_HPP_ 10 | 11 | #include "lightweight_filtering/common.hpp" 12 | #include "lightweight_filtering/ModelBase.hpp" 13 | #include "lightweight_filtering/PropertyHandler.hpp" 14 | 15 | namespace LWF{ 16 | 17 | template 18 | class Prediction: public ModelBase,typename FilterState::mtState,typename FilterState::mtState,typename FilterState::mtPredictionNoise>, public PropertyHandler{ 19 | public: 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | typedef ModelBase,typename FilterState::mtState,typename FilterState::mtState,typename FilterState::mtPredictionNoise> mtModelBase; 22 | typedef FilterState mtFilterState; 23 | typedef typename mtFilterState::mtState mtState; 24 | typedef typename mtModelBase::mtInputTuple mtInputTuple; 25 | typedef typename mtFilterState::mtPredictionMeas mtMeas; 26 | typedef typename mtFilterState::mtPredictionNoise mtNoise; 27 | mtMeas meas_; 28 | Eigen::MatrixXd prenoiP_; 29 | Eigen::MatrixXd prenoiPinv_; 30 | bool disablePreAndPostProcessingWarning_; 31 | Prediction(): prenoiP_((int)(mtNoise::D_),(int)(mtNoise::D_)), 32 | prenoiPinv_((int)(mtNoise::D_),(int)(mtNoise::D_)){ 33 | prenoiP_.setIdentity(); 34 | prenoiP_ *= 0.0001; 35 | mtNoise n; 36 | n.setIdentity(); 37 | n.registerCovarianceToPropertyHandler_(prenoiP_,this,"PredictionNoise."); 38 | disablePreAndPostProcessingWarning_ = false; 39 | refreshProperties(); 40 | }; 41 | virtual ~Prediction(){}; 42 | void refreshProperties(){ 43 | prenoiPinv_.setIdentity(); 44 | prenoiP_.llt().solveInPlace(prenoiPinv_); 45 | } 46 | void eval_(mtState& x, const mtInputTuple& inputs, double dt) const{ 47 | evalPrediction(x,std::get<0>(inputs),std::get<1>(inputs),dt); 48 | } 49 | template::type* = nullptr> 50 | void jacInput_(Eigen::MatrixXd& F, const mtInputTuple& inputs, double dt) const{ 51 | jacPreviousState(F,std::get<0>(inputs),dt); 52 | } 53 | template::type* = nullptr> 54 | void jacInput_(Eigen::MatrixXd& F, const mtInputTuple& inputs, double dt) const{ 55 | jacNoise(F,std::get<0>(inputs),dt); 56 | } 57 | virtual void evalPrediction(mtState& x, const mtState& previousState, const mtNoise& noise, double dt) const = 0; 58 | virtual void evalPredictionShort(mtState& x, const mtState& previousState, double dt) const{ 59 | mtNoise n; // TODO get static for Identity() 60 | n.setIdentity(); 61 | evalPrediction(x,previousState,n,dt); 62 | } 63 | virtual void jacPreviousState(Eigen::MatrixXd& F, const mtState& previousState, double dt) const = 0; 64 | virtual void jacNoise(Eigen::MatrixXd& F, const mtState& previousState, double dt) const = 0; 65 | virtual void noMeasCase(mtFilterState& filterState, mtMeas& meas, double dt){}; 66 | virtual void preProcess(mtFilterState& filterState, const mtMeas& meas, double dt){ 67 | if(!disablePreAndPostProcessingWarning_){ 68 | std::cout << "Warning: prediction preProcessing is not implemented!" << std::endl; 69 | } 70 | }; 71 | virtual void postProcess(mtFilterState& filterState, const mtMeas& meas, double dt){ 72 | if(!disablePreAndPostProcessingWarning_){ 73 | std::cout << "Warning: prediction postProcessing is not implemented!" << std::endl; 74 | } 75 | }; 76 | int performPrediction(mtFilterState& filterState, const mtMeas& meas, double dt){ 77 | switch(filterState.mode_){ 78 | case ModeEKF: 79 | return performPredictionEKF(filterState,meas,dt); 80 | case ModeUKF: 81 | return performPredictionUKF(filterState,meas,dt); 82 | case ModeIEKF: 83 | return performPredictionEKF(filterState,meas,dt); 84 | default: 85 | return performPredictionEKF(filterState,meas,dt); 86 | } 87 | } 88 | int performPrediction(mtFilterState& filterState, double dt){ 89 | mtMeas meas; 90 | meas.setIdentity(); 91 | noMeasCase(filterState,meas,dt); 92 | return performPrediction(filterState,meas,dt); 93 | } 94 | int performPredictionEKF(mtFilterState& filterState, const mtMeas& meas, double dt){ 95 | preProcess(filterState,meas,dt); 96 | meas_ = meas; 97 | this->jacPreviousState(filterState.F_,filterState.state_,dt); 98 | this->jacNoise(filterState.G_,filterState.state_,dt); 99 | this->evalPredictionShort(filterState.state_,filterState.state_,dt); 100 | filterState.cov_ = filterState.F_*filterState.cov_*filterState.F_.transpose() + filterState.G_*prenoiP_*filterState.G_.transpose(); 101 | filterState.state_.fix(); 102 | enforceSymmetry(filterState.cov_); 103 | filterState.t_ += dt; 104 | postProcess(filterState,meas,dt); 105 | return 0; 106 | } 107 | int performPredictionUKF(mtFilterState& filterState, const mtMeas& meas, double dt){ 108 | filterState.refreshNoiseSigmaPoints(prenoiP_); 109 | preProcess(filterState,meas,dt); 110 | meas_ = meas; 111 | filterState.stateSigmaPoints_.computeFromGaussian(filterState.state_,filterState.cov_); 112 | 113 | // Prediction 114 | for(unsigned int i=0;ievalPrediction(filterState.stateSigmaPointsPre_(i),filterState.stateSigmaPoints_(i),filterState.stateSigmaPointsNoi_(i),dt); 116 | } 117 | // Calculate mean and variance 118 | filterState.stateSigmaPointsPre_.getMean(filterState.state_); 119 | filterState.stateSigmaPointsPre_.getCovarianceMatrix(filterState.state_,filterState.cov_); 120 | filterState.state_.fix(); 121 | filterState.t_ += dt; 122 | postProcess(filterState,meas,dt); 123 | return 0; 124 | } 125 | int predictMerged(mtFilterState& filterState, double tTarget, const std::map& measMap){ 126 | switch(filterState.mode_){ 127 | case ModeEKF: 128 | return predictMergedEKF(filterState,tTarget,measMap); 129 | case ModeUKF: 130 | return predictMergedUKF(filterState,tTarget,measMap); 131 | case ModeIEKF: 132 | return predictMergedEKF(filterState,tTarget,measMap); 133 | default: 134 | return predictMergedEKF(filterState,tTarget,measMap); 135 | } 136 | } 137 | virtual int predictMergedEKF(mtFilterState& filterState, const double tTarget, const std::map& measMap){ 138 | const typename std::map::const_iterator itMeasStart = measMap.upper_bound(filterState.t_); 139 | if(itMeasStart == measMap.end()) return 0; 140 | typename std::map::const_iterator itMeasEnd = measMap.lower_bound(tTarget); 141 | if(itMeasEnd != measMap.end()) ++itMeasEnd; 142 | double dT = std::min(std::prev(itMeasEnd)->first,tTarget)-filterState.t_; 143 | if(dT <= 0) return 0; 144 | 145 | // Compute mean Measurement 146 | mtMeas meanMeas; 147 | typename mtMeas::mtDifVec vec; 148 | typename mtMeas::mtDifVec difVec; 149 | vec.setZero(); 150 | double t = itMeasStart->first; 151 | for(typename std::map::const_iterator itMeas=next(itMeasStart);itMeas!=itMeasEnd;itMeas++){ 152 | itMeasStart->second.boxMinus(itMeas->second,difVec); 153 | vec = vec + difVec*(std::min(itMeas->first,tTarget)-t); 154 | t = std::min(itMeas->first,tTarget); 155 | } 156 | vec = vec/dT; 157 | itMeasStart->second.boxPlus(vec,meanMeas); 158 | 159 | preProcess(filterState,meanMeas,dT); 160 | meas_ = meanMeas; 161 | this->jacPreviousState(filterState.F_,filterState.state_,dT); 162 | this->jacNoise(filterState.G_,filterState.state_,dT); // Works for time continuous parametrization of noise 163 | for(typename std::map::const_iterator itMeas=itMeasStart;itMeas!=itMeasEnd;itMeas++){ 164 | meas_ = itMeas->second; 165 | this->evalPredictionShort(filterState.state_,filterState.state_,std::min(itMeas->first,tTarget)-filterState.t_); 166 | filterState.t_ = std::min(itMeas->first,tTarget); 167 | } 168 | filterState.cov_ = filterState.F_*filterState.cov_*filterState.F_.transpose() + filterState.G_*prenoiP_*filterState.G_.transpose(); 169 | filterState.state_.fix(); 170 | enforceSymmetry(filterState.cov_); 171 | filterState.t_ = std::min(std::prev(itMeasEnd)->first,tTarget); 172 | postProcess(filterState,meanMeas,dT); 173 | return 0; 174 | } 175 | virtual int predictMergedUKF(mtFilterState& filterState, double tTarget, const std::map& measMap){ 176 | filterState.refreshNoiseSigmaPoints(prenoiP_); 177 | const typename std::map::const_iterator itMeasStart = measMap.upper_bound(filterState.t_); 178 | if(itMeasStart == measMap.end()) return 0; 179 | const typename std::map::const_iterator itMeasEnd = measMap.upper_bound(tTarget); 180 | if(itMeasEnd == measMap.begin()) return 0; 181 | double dT = std::prev(itMeasEnd)->first-filterState.t_; 182 | 183 | // Compute mean Measurement 184 | mtMeas meanMeas; 185 | typename mtMeas::mtDifVec vec; 186 | typename mtMeas::mtDifVec difVec; 187 | vec.setZero(); 188 | double t = itMeasStart->first; 189 | for(typename std::map::const_iterator itMeas=next(itMeasStart);itMeas!=itMeasEnd;itMeas++){ 190 | itMeasStart->second.boxMinus(itMeas->second,difVec); 191 | vec = vec + difVec*(itMeas->first-t); 192 | t = itMeas->first; 193 | } 194 | vec = vec/dT; 195 | itMeasStart->second.boxPlus(vec,meanMeas); 196 | 197 | preProcess(filterState,meanMeas,dT); 198 | meas_ = meanMeas; 199 | filterState.stateSigmaPoints_.computeFromGaussian(filterState.state_,filterState.cov_); 200 | 201 | // Prediction 202 | for(unsigned int i=0;ievalPrediction(filterState.stateSigmaPointsPre_(i),filterState.stateSigmaPoints_(i),filterState.stateSigmaPointsNoi_(i),dT); 204 | } 205 | filterState.stateSigmaPointsPre_.getMean(filterState.state_); 206 | filterState.stateSigmaPointsPre_.getCovarianceMatrix(filterState.state_,filterState.cov_); 207 | filterState.state_.fix(); 208 | filterState.t_ = std::prev(itMeasEnd)->first; 209 | postProcess(filterState,meanMeas,dT); 210 | return 0; 211 | } 212 | bool testPredictionJacs(double d = 1e-6,double th = 1e-6,double dt = 0.1){ 213 | mtState state; 214 | mtMeas meas; 215 | unsigned int s = 1; 216 | state.setRandom(s); 217 | meas.setRandom(s); 218 | return testPredictionJacs(state,meas,d,th,dt); 219 | } 220 | bool testPredictionJacs(const mtState& state, const mtMeas& meas, double d = 1e-6,double th = 1e-6,double dt = 0.1){ 221 | mtInputTuple inputs; 222 | std::get<0>(inputs) = state; 223 | std::get<1>(inputs).setIdentity(); // Noise is always set to zero for Jacobians 224 | meas_ = meas; 225 | return this->testJacs(inputs,d,th,dt); 226 | } 227 | }; 228 | 229 | } 230 | 231 | #endif /* LWF_PREDICTIONMODEL_HPP_ */ 232 | -------------------------------------------------------------------------------- /include/lightweight_filtering/PropertyHandler.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PropertyHandler.hpp 3 | * 4 | * Created on: Oct 27, 2014 5 | * Author: Bloeschm 6 | */ 7 | 8 | #ifndef LWF_PropertyHandler_HPP_ 9 | #define LWF_PropertyHandler_HPP_ 10 | 11 | #include 12 | #include 13 | #include "lightweight_filtering/common.hpp" 14 | #include 15 | #include 16 | 17 | namespace rot = kindr; 18 | 19 | namespace LWF{ 20 | 21 | template 22 | class Register{ 23 | public: 24 | typedef boost::property_tree::ptree ptree; 25 | Register(){}; 26 | virtual ~Register(){}; 27 | std::map registerMap_; 28 | std::unordered_set zeros_; 29 | void registerZero(TYPE& var){ 30 | if(zeros_.count(&var)!=0) std::cout << "Property Handler Error: Zero variable already registered." << std::endl; 31 | zeros_.insert(&var); 32 | } 33 | void registerScalar(std::string str, TYPE& var){ 34 | if(registerMap_.count(&var)!=0) std::cout << "Property Handler Error: Variable already registered to " << str << "." << std::endl; 35 | registerMap_[&var] = str; 36 | } 37 | void registerVector(std::string str, Eigen::Matrix& var){ 38 | registerScalar(str + "_x",var(0)); 39 | registerScalar(str + "_y",var(1)); 40 | registerScalar(str + "_z",var(2)); 41 | } 42 | void registerQuaternion(std::string str, rot::RotationQuaternion& var){ 43 | registerScalar(str + "_w",var.toImplementation().w()); 44 | registerScalar(str + "_x",var.toImplementation().x()); 45 | registerScalar(str + "_y",var.toImplementation().y()); 46 | registerScalar(str + "_z",var.toImplementation().z()); 47 | } 48 | template 49 | void registerMatrix(std::string str, Eigen::Matrix& var){ 50 | for(unsigned int i=0;i 57 | void registerDiagonalMatrix(std::string str, Eigen::MatrixBase& var){ 58 | const int N = var.rows(); 59 | for(unsigned int i=0;i 69 | void registerScaledUnitMatrix(std::string str, Eigen::Matrix& var){ 70 | for(unsigned int i=0;isecond == str){ 83 | registerMap_.erase(it); 84 | found = true; 85 | break; 86 | } 87 | } 88 | if(!found) std::cout << "Property Handler Error: Cannot remove variable with str = " << str << std::endl; 89 | } 90 | void removeScalarByVar(TYPE& var){ 91 | if(registerMap_.count(&var)==0) std::cout << "Property Handler Error: Cannot remove variable, does not exist!" << std::endl; 92 | registerMap_.erase(&var); 93 | } 94 | void buildPropertyTree(ptree& pt){ 95 | for(typename std::map::iterator it=registerMap_.begin(); it != registerMap_.end(); ++it){ 96 | pt.put(it->second, *(it->first)); 97 | } 98 | } 99 | void readPropertyTree(ptree& pt){ 100 | for(typename std::map::iterator it=registerMap_.begin(); it != registerMap_.end(); ++it){ 101 | *(it->first) = pt.get(it->second); 102 | } 103 | for(typename std::unordered_set::iterator it=zeros_.begin(); it != zeros_.end(); ++it){ 104 | **it = static_cast(0); 105 | } 106 | } 107 | }; 108 | 109 | class PropertyHandler{ 110 | public: 111 | typedef boost::property_tree::ptree ptree; 112 | PropertyHandler(){}; 113 | virtual ~PropertyHandler(){}; 114 | Register boolRegister_; 115 | Register intRegister_; 116 | Register doubleRegister_; 117 | Register stringRegister_; 118 | std::unordered_map subHandlers_; 119 | void buildPropertyTree(ptree& pt){ 120 | boolRegister_.buildPropertyTree(pt); 121 | intRegister_.buildPropertyTree(pt); 122 | doubleRegister_.buildPropertyTree(pt); 123 | stringRegister_.buildPropertyTree(pt); 124 | for(typename std::unordered_map::iterator it=subHandlers_.begin(); it != subHandlers_.end(); ++it){ 125 | ptree ptsub; 126 | it->second->buildPropertyTree(ptsub); 127 | pt.add_child(it->first,ptsub); 128 | } 129 | } 130 | void readPropertyTree(ptree& pt){ 131 | boolRegister_.readPropertyTree(pt); 132 | intRegister_.readPropertyTree(pt); 133 | doubleRegister_.readPropertyTree(pt); 134 | stringRegister_.readPropertyTree(pt); 135 | for(typename std::unordered_map::iterator it=subHandlers_.begin(); it != subHandlers_.end(); ++it){ 136 | ptree ptsub; 137 | ptsub = pt.get_child(it->first); 138 | it->second->readPropertyTree(ptsub); 139 | } 140 | } 141 | void registerSubHandler(std::string str,PropertyHandler& subHandler){ 142 | if(subHandlers_.count(str)!=0) std::cout << "Property Handler Error: subHandler with name " << str << " already exists" << std::endl; 143 | subHandlers_[str] = &subHandler; 144 | } 145 | void writeToInfo(const std::string &filename){ 146 | ptree pt; 147 | buildPropertyTree(pt); 148 | write_info(filename,pt); 149 | } 150 | void readFromInfo(const std::string &filename){ 151 | ptree ptDefault; 152 | buildPropertyTree(ptDefault); 153 | ptree pt; 154 | try{ 155 | read_info(filename,pt); 156 | readPropertyTree(pt); 157 | refreshProperties(); 158 | for(typename std::unordered_map::iterator it=subHandlers_.begin(); it != subHandlers_.end(); ++it){ 159 | it->second->refreshProperties(); 160 | } 161 | } catch (boost::property_tree::ptree_error& e){ 162 | std::cout << "An exception occurred. " << e.what() << std::endl; 163 | std::string newFilename = filename + "_new"; 164 | std::cout << "\033[31mWriting a new info-file to " << newFilename << "\033[0m" << std::endl; 165 | write_info(newFilename,ptDefault); 166 | refreshProperties(); 167 | for(typename std::unordered_map::iterator it=subHandlers_.begin(); it != subHandlers_.end(); ++it){ 168 | it->second->refreshProperties(); 169 | } 170 | } 171 | } 172 | virtual void refreshProperties(){}; 173 | }; 174 | 175 | } 176 | 177 | #endif /* LWF_PropertyHandler_HPP_ */ 178 | -------------------------------------------------------------------------------- /include/lightweight_filtering/SigmaPoints.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SigmaPoints.hpp 3 | * 4 | * Created on: Feb 9, 2014 5 | * Author: Bloeschm 6 | */ 7 | 8 | #ifndef LWF_SIGMAPOINTS_HPP_ 9 | #define LWF_SIGMAPOINTS_HPP_ 10 | 11 | #include "lightweight_filtering/State.hpp" 12 | #include "lightweight_filtering/common.hpp" 13 | 14 | namespace LWF{ 15 | 16 | template 17 | class SigmaPoints{ 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | typedef State mtState; 21 | static const unsigned int N_ = N; 22 | static const unsigned int L_ = L; 23 | static const unsigned int O_ = O; 24 | double wm_ = 1.0; 25 | double wc_ = 1.0; 26 | double wc0_ = 1.0; 27 | double gamma_ = 1.0; 28 | mtState sigmaPoints_[N]; 29 | Eigen::MatrixXd S_; 30 | SigmaPoints(){ 31 | static_assert(N_+O_<=L_, "Bad dimensions for sigmapoints"); 32 | S_.setZero(); 33 | }; 34 | virtual ~SigmaPoints(){}; 35 | void getMean(mtState& mean) const{ 36 | typename mtState::mtDifVec vec; 37 | typename mtState::mtDifVec vecTemp; 38 | vec.setZero(); 39 | for(unsigned int i=1;i 63 | void getCovarianceMatrix(const SigmaPoints& sigmaPoints2, Eigen::MatrixXd& C) const{ 64 | mtState mean1; 65 | getMean(mean1); 66 | State2 mean2; 67 | sigmaPoints2.getMean(mean2); 68 | getCovarianceMatrix(sigmaPoints2,mean1,mean2,C); 69 | }; 70 | template 71 | void getCovarianceMatrix(const SigmaPoints& sigmaPoints2, const mtState& mean1, const State2& mean2, Eigen::MatrixXd& C) const{ 72 | typename mtState::mtDifVec vec1; 73 | typename State2::mtDifVec vec2; 74 | Eigen::MatrixXd dynVec1; 75 | Eigen::MatrixXd dynVec2; 76 | (*this)(0).boxMinus(mean1,vec1); 77 | sigmaPoints2(0).boxMinus(mean2,vec2); 78 | dynVec1 = vec1; 79 | dynVec2 = vec2; 80 | C = dynVec1*dynVec2.transpose()*wc0_; 81 | for(unsigned int i=1;i 90 | void extendZeroMeanGaussian(const SigmaPoints& sigmaPoints2, const Eigen::MatrixXd& P, const Eigen::MatrixXd& Q){ // samples the last dimensions 91 | Eigen::MatrixXd C = Q.transpose()*sigmaPoints2.S_.inverse(); 92 | Eigen::LDLT ldltOfP(P-Q.transpose()*sigmaPoints2.S_.transpose().inverse()*sigmaPoints2.S_.inverse()*Q); 93 | Eigen::MatrixXd ldltL = ldltOfP.matrixL(); 94 | Eigen::MatrixXd ldltD = ldltOfP.vectorD().asDiagonal(); 95 | for(unsigned int i=0;i0){ 97 | ldltD(i,i) = std::sqrt(ldltD(i,i)); 98 | } else if(ldltD(i,i)==0) { 99 | ldltD(i,i) = 0.0; 100 | std::cout << "CAUTION: Covariance matrix is only positive SEMIdefinite" << std::endl; 101 | } else { 102 | ldltD(i,i) = 0.0; 103 | std::cout << "ERROR: Covariance matrix is not positive semidefinite" << std::endl; 104 | } 105 | } 106 | if(ldltOfP.info()==Eigen::NumericalIssue) std::cout << "Numerical issues while computing Cholesky Matrix" << std::endl; 107 | S_ = ldltOfP.transpositionsP().transpose()*ldltL*ldltD; 108 | 109 | sigmaPoints_[0].setIdentity(); 110 | int otherSize = (N_-2*mtState::D_-1)/2; 111 | for(unsigned int i=0;i ldltOfP(P); 123 | Eigen::MatrixXd ldltL = ldltOfP.matrixL(); 124 | Eigen::MatrixXd ldltD = ldltOfP.vectorD().asDiagonal(); 125 | for(unsigned int i=0;i0){ 127 | ldltD(i,i) = std::sqrt(ldltD(i,i)); 128 | } else if(ldltD(i,i)==0) { 129 | ldltD(i,i) = 0.0; 130 | std::cout << "CAUTION: Covariance matrix is only positive SEMIdefinite" << std::endl; 131 | } else { 132 | ldltD(i,i) = 0.0; 133 | std::cout << "ERROR: Covariance matrix is not positive semidefinite" << std::endl; 134 | } 135 | } 136 | if(ldltOfP.info()==Eigen::NumericalIssue) std::cout << "Numerical issues while computing Cholesky Matrix" << std::endl; 137 | S_ = ldltOfP.transpositionsP().transpose()*ldltL*ldltD; 138 | 139 | sigmaPoints_[0] = mean; 140 | for(unsigned int i=0;i ldltOfP(Q.transpose()*P*Q); // 149 | Eigen::MatrixXd ldltL = ldltOfP.matrixL(); 150 | Eigen::MatrixXd ldltD = ldltOfP.vectorD().asDiagonal(); 151 | for(unsigned int i=0;i0){ 153 | ldltD(i,i) = std::sqrt(ldltD(i,i)); 154 | } else if(ldltD(i,i)==0) { 155 | ldltD(i,i) = 0.0; 156 | std::cout << "CAUTION: Covariance matrix is only positive SEMIdefinite" << std::endl; 157 | } else { 158 | ldltD(i,i) = 0.0; 159 | std::cout << "ERROR: Covariance matrix is not positive semidefinite" << std::endl; 160 | } 161 | } 162 | if(ldltOfP.info()==Eigen::NumericalIssue) std::cout << "Numerical issues while computing Cholesky Matrix" << std::endl; 163 | S_ = Q*ldltOfP.transpositionsP().transpose()*ldltL*ldltD; 164 | 165 | sigmaPoints_[0] = mean; 166 | for(unsigned int i=0;i 14 | 15 | namespace rot = kindr; 16 | 17 | namespace LWF{ 18 | 19 | template 20 | class ElementBase{ 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | ElementBase(){}; 24 | virtual ~ElementBase(){}; 25 | static const unsigned int D_ = D; 26 | static const unsigned int E_ = E; 27 | typedef Eigen::Matrix mtDifVec; 28 | typedef GET mtGet; 29 | std::string name_; 30 | virtual void boxPlus(const mtDifVec& vecIn, DERIVED& stateOut) const = 0; 31 | virtual void boxMinus(const DERIVED& stateIn, mtDifVec& vecOut) const = 0; 32 | virtual void boxMinusJac(const DERIVED& stateIn, MXD& matOut) const = 0; 33 | virtual void print() const = 0; 34 | virtual void setIdentity() = 0; 35 | virtual void setRandom(unsigned int& s) = 0; 36 | virtual void fix() = 0; 37 | static DERIVED Identity(){ 38 | DERIVED identity; 39 | identity.setIdentity(); 40 | return identity; 41 | } 42 | DERIVED& operator=(DERIVED other){ 43 | other.swap(*this); 44 | return *this; 45 | } 46 | virtual mtGet& get(unsigned int i) = 0; 47 | virtual const mtGet& get(unsigned int i) const = 0; 48 | virtual void registerElementToPropertyHandler(PropertyHandler* mpPropertyHandler, const std::string& str) = 0; 49 | void registerCovarianceToPropertyHandler(Eigen::MatrixXd& cov, PropertyHandler* mpPropertyHandler, const std::string& str, int j){ 50 | assert(j+D_<=cov.cols()); 51 | for(unsigned int i=0;idoubleRegister_.registerScalar(str + name_ + "_" + std::to_string(i), cov(j+i,j+i)); 53 | } 54 | } 55 | }; 56 | 57 | template 58 | class AuxiliaryBase: public ElementBase,DERIVED,0>{ 59 | public: 60 | typedef ElementBase,DERIVED,0> Base; 61 | using typename Base::mtDifVec; 62 | using typename Base::mtGet; 63 | AuxiliaryBase(){} 64 | AuxiliaryBase(const AuxiliaryBase& other){} 65 | virtual ~AuxiliaryBase(){} 66 | virtual void boxPlus(const mtDifVec& vecIn, AuxiliaryBase& stateOut) const{ 67 | static_cast(stateOut) = static_cast(*this); 68 | } 69 | virtual void boxMinus(const AuxiliaryBase& stateIn, mtDifVec& vecOut) const{} 70 | virtual void boxMinusJac(const AuxiliaryBase& stateIn, MXD& matOut) const{} 71 | virtual void print() const{} 72 | virtual void setIdentity(){} 73 | virtual void setRandom(unsigned int& s){} 74 | virtual void fix(){} 75 | mtGet& get(unsigned int i){ 76 | return static_cast(*this); 77 | } 78 | const mtGet& get(unsigned int i) const{ 79 | return static_cast(*this); 80 | } 81 | virtual void registerElementToPropertyHandler(PropertyHandler* mpPropertyHandler, const std::string& str){} 82 | }; 83 | 84 | class ScalarElement: public ElementBase{ 85 | public: 86 | double s_; 87 | ScalarElement(){} 88 | ScalarElement(const ScalarElement& other){ 89 | s_ = other.s_; 90 | } 91 | virtual ~ScalarElement(){}; 92 | void boxPlus(const mtDifVec& vecIn, ScalarElement& stateOut) const{ 93 | stateOut.s_ = s_ + vecIn(0); 94 | } 95 | void boxMinus(const ScalarElement& stateIn, mtDifVec& vecOut) const{ 96 | vecOut(0) = s_ - stateIn.s_; 97 | } 98 | void boxMinusJac(const ScalarElement& stateIn, MXD& matOut) const{ 99 | matOut.setIdentity(); 100 | } 101 | void print() const{ 102 | std::cout << s_ << std::endl; 103 | } 104 | void setIdentity(){ 105 | s_ = 0.0; 106 | } 107 | void setRandom(unsigned int& s){ 108 | std::default_random_engine generator (s); 109 | std::normal_distribution distribution (0.0,1.0); 110 | s_ = distribution(generator); 111 | s++; 112 | } 113 | void fix(){ 114 | } 115 | mtGet& get(unsigned int i = 0){ 116 | assert(i==0); 117 | return s_; 118 | } 119 | const mtGet& get(unsigned int i = 0) const{ 120 | assert(i==0); 121 | return s_; 122 | } 123 | void registerElementToPropertyHandler(PropertyHandler* mpPropertyHandler, const std::string& str){ 124 | mpPropertyHandler->doubleRegister_.registerScalar(str + name_,s_); 125 | } 126 | }; 127 | 128 | template 129 | class VectorElement: public ElementBase,Eigen::Matrix,N>{ 130 | public: 131 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 132 | typedef ElementBase,Eigen::Matrix,N> Base; 133 | using typename Base::mtDifVec; 134 | using typename Base::mtGet; 135 | using Base::name_; 136 | static const unsigned int N_ = N; 137 | Eigen::Matrix v_; 138 | VectorElement(){} 139 | VectorElement(const VectorElement& other){ 140 | v_ = other.v_; 141 | } 142 | virtual ~VectorElement(){}; 143 | void boxPlus(const mtDifVec& vecIn, VectorElement& stateOut) const{ 144 | stateOut.v_ = v_ + vecIn; 145 | } 146 | void boxMinus(const VectorElement& stateIn, mtDifVec& vecOut) const{ 147 | vecOut = v_ - stateIn.v_; 148 | } 149 | void boxMinusJac(const VectorElement& stateIn, MXD& matOut) const{ 150 | matOut.setIdentity(); 151 | } 152 | void print() const{ 153 | std::cout << v_.transpose() << std::endl; 154 | } 155 | void setIdentity(){ 156 | v_.setZero(); 157 | } 158 | void setRandom(unsigned int& s){ 159 | std::default_random_engine generator (s); 160 | std::normal_distribution distribution (0.0,1.0); 161 | for(unsigned int i=0;idoubleRegister_.registerScalar(str + name_ + "_" + std::to_string(i), v_(i)); 179 | } 180 | } 181 | }; 182 | 183 | class QuaternionElement: public ElementBase{ 184 | public: 185 | QPD q_; 186 | QuaternionElement(){} 187 | QuaternionElement(const QuaternionElement& other){ 188 | q_ = other.q_; 189 | } 190 | virtual ~QuaternionElement(){}; 191 | void boxPlus(const mtDifVec& vecIn, QuaternionElement& stateOut) const{ 192 | stateOut.q_ = q_.boxPlus(vecIn); 193 | } 194 | void boxMinus(const QuaternionElement& stateIn, mtDifVec& vecOut) const{ 195 | vecOut = q_.boxMinus(stateIn.q_); 196 | } 197 | void boxMinusJac(const QuaternionElement& stateIn, MXD& matOut) const{ 198 | mtDifVec diff; 199 | boxMinus(stateIn,diff); 200 | matOut = Lmat(diff).inverse(); 201 | } 202 | void print() const{ 203 | std::cout << q_ << std::endl; 204 | } 205 | void setIdentity(){ 206 | q_.setIdentity(); 207 | } 208 | void setRandom(unsigned int& s){ 209 | std::default_random_engine generator (s); 210 | std::normal_distribution distribution (0.0,1.0); 211 | q_.toImplementation().w() = distribution(generator); 212 | q_.toImplementation().x() = distribution(generator); 213 | q_.toImplementation().y() = distribution(generator); 214 | q_.toImplementation().z() = distribution(generator); 215 | fix(); 216 | s++; 217 | } 218 | void fix(){ 219 | q_.fix(); 220 | } 221 | mtGet& get(unsigned int i = 0){ 222 | assert(i==0); 223 | return q_; 224 | } 225 | const mtGet& get(unsigned int i = 0) const{ 226 | assert(i==0); 227 | return q_; 228 | } 229 | void registerElementToPropertyHandler(PropertyHandler* mpPropertyHandler, const std::string& str){ 230 | mpPropertyHandler->doubleRegister_.registerQuaternion(str + name_, q_); 231 | } 232 | }; 233 | 234 | class NormalVectorElement: public ElementBase{ 235 | public: 236 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 237 | QPD q_; 238 | const V3D e_x; 239 | const V3D e_y; 240 | const V3D e_z; 241 | NormalVectorElement(): e_x(1,0,0), e_y(0,1,0), e_z(0,0,1){} 242 | NormalVectorElement(const NormalVectorElement& other): e_x(1,0,0), e_y(0,1,0), e_z(0,0,1){ 243 | q_ = other.q_; 244 | } 245 | NormalVectorElement(const V3D& vec): e_x(1,0,0), e_y(0,1,0), e_z(0,0,1){ 246 | setFromVector(vec); 247 | } 248 | NormalVectorElement(const QPD& q): e_x(1,0,0), e_y(0,1,0), e_z(0,0,1){ 249 | q_ = q; 250 | } 251 | virtual ~NormalVectorElement(){}; 252 | V3D getVec() const{ 253 | return q_.rotate(e_z); 254 | } 255 | V3D getPerp1() const{ 256 | return q_.rotate(e_x); 257 | } 258 | V3D getPerp2() const{ 259 | return q_.rotate(e_y); 260 | } 261 | NormalVectorElement& operator=(const NormalVectorElement& other){ 262 | q_ = other.q_; 263 | return *this; 264 | } 265 | static V3D getRotationFromTwoNormals(const V3D& a, const V3D& b, const V3D& a_perp){ 266 | const V3D cross = a.cross(b); 267 | const double crossNorm = cross.norm(); 268 | const double c = a.dot(b); 269 | const double angle = std::acos(c); 270 | if(crossNorm<1e-6){ 271 | if(c>0){ 272 | return cross; 273 | } else { 274 | return a_perp*M_PI; 275 | } 276 | } else { 277 | return cross*(angle/crossNorm); 278 | } 279 | } 280 | static V3D getRotationFromTwoNormals(const NormalVectorElement& a, const NormalVectorElement& b){ 281 | return getRotationFromTwoNormals(a.getVec(),b.getVec(),a.getPerp1()); 282 | } 283 | static M3D getRotationFromTwoNormalsJac(const V3D& a, const V3D& b){ 284 | const V3D cross = a.cross(b); 285 | const double crossNorm = cross.norm(); 286 | V3D crossNormalized = cross/crossNorm; 287 | M3D crossNormalizedSqew = gSM(crossNormalized); 288 | const double c = a.dot(b); 289 | const double angle = std::acos(c); 290 | if(crossNorm<1e-6){ 291 | if(c>0){ 292 | return -gSM(b); 293 | } else { 294 | return M3D::Zero(); 295 | } 296 | } else { 297 | return -1/crossNorm*(crossNormalized*b.transpose()-(crossNormalizedSqew*crossNormalizedSqew*gSM(b)*angle)); 298 | } 299 | } 300 | static M3D getRotationFromTwoNormalsJac(const NormalVectorElement& a, const NormalVectorElement& b){ 301 | return getRotationFromTwoNormalsJac(a.getVec(),b.getVec()); 302 | } 303 | void setFromVector(V3D vec){ 304 | const double d = vec.norm(); 305 | if(d > 1e-6){ 306 | vec = vec/d; 307 | q_ = q_.exponentialMap(getRotationFromTwoNormals(e_z,vec,e_x)); 308 | } else { 309 | q_.setIdentity(); 310 | } 311 | } 312 | NormalVectorElement rotated(const QPD& q) const{ 313 | return NormalVectorElement(q*q_); 314 | } 315 | NormalVectorElement inverted() const{ 316 | QPD q = q.exponentialMap(M_PI*getPerp1()); 317 | return NormalVectorElement(q*q_); 318 | } 319 | void boxPlus(const mtDifVec& vecIn, NormalVectorElement& stateOut) const{ 320 | QPD q = q.exponentialMap(vecIn(0)*getPerp1()+vecIn(1)*getPerp2()); 321 | stateOut.q_ = q*q_; 322 | } 323 | void boxMinus(const NormalVectorElement& stateIn, mtDifVec& vecOut) const{ 324 | vecOut = stateIn.getN().transpose()*getRotationFromTwoNormals(stateIn,*this); 325 | } 326 | void boxMinusJac(const NormalVectorElement& stateIn, MXD& matOut) const{ 327 | matOut = -stateIn.getN().transpose()*getRotationFromTwoNormalsJac(*this,stateIn)*this->getM(); 328 | } 329 | void print() const{ 330 | std::cout << getVec().transpose() << std::endl; 331 | } 332 | void setIdentity(){ 333 | q_.setIdentity(); 334 | } 335 | void setRandom(unsigned int& s){ 336 | std::default_random_engine generator (s); 337 | std::normal_distribution distribution (0.0,1.0); 338 | q_.toImplementation().w() = distribution(generator); 339 | q_.toImplementation().x() = distribution(generator); 340 | q_.toImplementation().y() = distribution(generator); 341 | q_.toImplementation().z() = distribution(generator); 342 | q_.fix(); 343 | s++; 344 | } 345 | void fix(){ 346 | q_.fix(); 347 | } 348 | mtGet& get(unsigned int i = 0){ 349 | assert(i==0); 350 | return *this; 351 | } 352 | const mtGet& get(unsigned int i = 0) const{ 353 | assert(i==0); 354 | return *this; 355 | } 356 | void registerElementToPropertyHandler(PropertyHandler* mpPropertyHandler, const std::string& str){ 357 | mpPropertyHandler->doubleRegister_.registerQuaternion(str + name_, q_); 358 | } 359 | Eigen::Matrix getM() const { 360 | Eigen::Matrix M; 361 | M.col(0) = -getPerp2(); 362 | M.col(1) = getPerp1(); 363 | return M; 364 | } 365 | Eigen::Matrix getN() const { 366 | Eigen::Matrix M; 367 | M.col(0) = getPerp1(); 368 | M.col(1) = getPerp2(); 369 | return M; 370 | } 371 | }; 372 | 373 | template 374 | class ArrayElement: public ElementBase,typename Element::mtGet,M*Element::D_,Element::D_>{ 375 | public: 376 | typedef ElementBase,typename Element::mtGet,M*Element::D_,Element::D_> Base; 377 | using typename Base::mtDifVec; 378 | using typename Base::mtGet; 379 | using Base::name_; 380 | using Base::D_; 381 | static const unsigned int M_ = M; 382 | Element array_[M_]; 383 | mutable MXD boxMinusJacMat_; 384 | ArrayElement(): boxMinusJacMat_((int)Element::D_,(int)Element::D_){ 385 | for(unsigned int i=0; i0), typename std::enable_if::type* = nullptr> 399 | void boxPlus_(const mtDifVec& vecIn, ArrayElement& stateOut) const{ 400 | for(unsigned int i=0; i(Element::D_*i,0),stateOut.array_[i]); 402 | } 403 | } 404 | template0), typename std::enable_if::type* = nullptr> 405 | void boxPlus_(const mtDifVec& vecIn, ArrayElement& stateOut) const{} 406 | void boxMinus(const ArrayElement& stateIn, mtDifVec& vecOut) const{ 407 | boxMinus_(stateIn,vecOut); 408 | } 409 | template0), typename std::enable_if::type* = nullptr> 410 | void boxMinus_(const ArrayElement& stateIn, mtDifVec& vecOut) const{ 411 | typename Element::mtDifVec difVec; 412 | for(unsigned int i=0; i(Element::D_*i,0) = difVec; 415 | } 416 | } 417 | template0), typename std::enable_if::type* = nullptr> 418 | void boxMinus_(const ArrayElement& stateIn, mtDifVec& vecOut) const{} 419 | void boxMinusJac(const ArrayElement& stateIn, MXD& matOut) const{ 420 | boxMinusJac_(stateIn,matOut); 421 | } 422 | template0), typename std::enable_if::type* = nullptr> 423 | void boxMinusJac_(const ArrayElement& stateIn, MXD& matOut) const{ 424 | matOut.setZero(); 425 | for(unsigned int i=0; i(Element::D_*i,Element::D_*i) = boxMinusJacMat_; 428 | } 429 | } 430 | template0), typename std::enable_if::type* = nullptr> 431 | void boxMinusJac_(const ArrayElement& stateIn, MXD& matOut) const{} 432 | void print() const{ 433 | for(unsigned int i=0; i 468 | class TH_convert{ 469 | public: 470 | typedef std::tuple t; 471 | }; 472 | 473 | template 474 | class TH_multiple_elements{ 475 | private: 476 | public: 477 | typedef decltype(std::tuple_cat(typename TH_convert::t(),typename TH_multiple_elements::t())) t; 478 | }; 479 | template 480 | class TH_multiple_elements{ 481 | public: 482 | typedef typename TH_convert::t t; 483 | }; 484 | template 485 | class TH_multiple_elements{ 486 | public: 487 | typedef std::tuple<> t; 488 | }; 489 | 490 | template 491 | class TH_getDimension{ 492 | public: 493 | static const unsigned int D_ = Element::D_; 494 | }; 495 | template 496 | class TH_getDimension>{ 497 | public: 498 | static const unsigned int D_ = Element::D_; 499 | }; 500 | template 501 | class TH_getDimension>{ 502 | public: 503 | static const unsigned int D_ = Element::D_ + TH_getDimension>::D_; 504 | }; 505 | 506 | template 507 | class State{ 508 | public: 509 | typedef decltype(std::tuple_cat(typename TH_convert::t()...)) t; 510 | static const unsigned int D_ = TH_getDimension::D_; 511 | static const unsigned int E_ = std::tuple_size::value; 512 | typedef Eigen::Matrix mtDifVec; 513 | t mElements_; 514 | State(){ 515 | createDefaultNames(); 516 | } 517 | State(const State& other): mElements_(other.mElements_){ 518 | } 519 | virtual ~State(){}; 520 | void boxPlus(const mtDifVec& vecIn, State& stateOut) const{ 521 | boxPlus_(vecIn,stateOut); 522 | } 523 | template::type* = nullptr> 524 | inline void boxPlus_(const mtDifVec& vecIn, State& stateOut) const{ 525 | if(std::tuple_element::type::D_>0){ 526 | std::get(mElements_).boxPlus(vecIn.template block::type::D_,1>(j,0),std::get(stateOut.mElements_)); 527 | } else { // Required for auxiliary states 528 | Eigen::Matrix::type::D_,1> dummyVec; 529 | std::get(mElements_).boxPlus(dummyVec,std::get(stateOut.mElements_)); 530 | } 531 | boxPlus_::type::D_>(vecIn,stateOut); 532 | } 533 | template=E_)>::type* = nullptr> 534 | inline void boxPlus_(const mtDifVec& vecIn, State& stateOut) const{} 535 | void boxMinus(const State& stateIn, mtDifVec& vecOut) const{ 536 | boxMinus_(stateIn,vecOut); 537 | } 538 | template::type* = nullptr> 539 | inline void boxMinus_(const State& stateIn, mtDifVec& vecOut) const{ 540 | if(std::tuple_element::type::D_>0){ 541 | typename std::tuple_element::type::mtDifVec difVec; 542 | std::get(mElements_).boxMinus(std::get(stateIn.mElements_),difVec); 543 | vecOut.template block::type::D_,1>(j,0) = difVec; 544 | } 545 | boxMinus_::type::D_>(stateIn,vecOut); 546 | } 547 | template=E_)>::type* = nullptr> 548 | inline void boxMinus_(const State& stateIn, mtDifVec& vecOut) const{} 549 | void boxMinusJac(const State& stateIn, MXD& matOut) const{ 550 | matOut.setZero(); 551 | boxMinusJac_(stateIn,matOut); 552 | } 553 | template::type* = nullptr> 554 | inline void boxMinusJac_(const State& stateIn, MXD& matOut) const{ 555 | if(std::tuple_element::type::D_>0){ 556 | MXD mat((int)std::tuple_element::type::D_,(int)std::tuple_element::type::D_); 557 | std::get(mElements_).boxMinusJac(std::get(stateIn.mElements_),mat); 558 | matOut.template block::type::D_,std::tuple_element::type::D_>(j,j) = mat; 559 | } 560 | boxMinusJac_::type::D_>(stateIn,matOut); 561 | } 562 | template=E_)>::type* = nullptr> 563 | inline void boxMinusJac_(const State& stateIn, MXD& matOut) const{} 564 | void print() const{ 565 | print_(); 566 | } 567 | template::type* = nullptr> 568 | inline void print_() const{ 569 | std::get(mElements_).print(); 570 | print_(); 571 | } 572 | template=E_)>::type* = nullptr> 573 | inline void print_() const{} 574 | void setIdentity(){ 575 | setIdentity_(); 576 | } 577 | template::type* = nullptr> 578 | inline void setIdentity_(){ 579 | std::get(mElements_).setIdentity(); 580 | setIdentity_(); 581 | } 582 | template=E_)>::type* = nullptr> 583 | inline void setIdentity_(){} 584 | void setRandom(unsigned int& s){ 585 | setRandom_(s); 586 | } 587 | template::type* = nullptr> 588 | inline void setRandom_(unsigned int& s){ 589 | std::get(mElements_).setRandom(s); 590 | setRandom_(s); 591 | } 592 | template=E_)>::type* = nullptr> 593 | inline void setRandom_(unsigned int& s){} 594 | void fix(){ 595 | fix_(); 596 | } 597 | template::type* = nullptr> 598 | inline void fix_(){ 599 | std::get(mElements_).fix(); 600 | fix_(); 601 | } 602 | template=E_)>::type* = nullptr> 603 | inline void fix_(){} 604 | void registerElementsToPropertyHandler(PropertyHandler* mtPropertyHandler, const std::string& str){ 605 | registerElementsToPropertyHandler_(mtPropertyHandler,str); 606 | } 607 | template::type* = nullptr> 608 | inline void registerElementsToPropertyHandler_(PropertyHandler* mtPropertyHandler, const std::string& str){ 609 | std::get(mElements_).registerElementToPropertyHandler(mtPropertyHandler,str); 610 | registerElementsToPropertyHandler_(mtPropertyHandler,str); 611 | } 612 | template=E_)>::type* = nullptr> 613 | inline void registerElementsToPropertyHandler_(PropertyHandler* mtPropertyHandler, const std::string& str){} 614 | void registerCovarianceToPropertyHandler(Eigen::MatrixXd& cov, PropertyHandler* mpPropertyHandler, const std::string& str){ 615 | registerCovarianceToPropertyHandler_(cov,mpPropertyHandler,str); 616 | } 617 | template::type* = nullptr> 618 | inline void registerCovarianceToPropertyHandler_(Eigen::MatrixXd& cov, PropertyHandler* mpPropertyHandler, const std::string& str, int j=0){ 619 | std::get(mElements_).template registerCovarianceToPropertyHandler(cov,mpPropertyHandler,str,j); 620 | registerCovarianceToPropertyHandler_(cov,mpPropertyHandler,str,j+std::tuple_element::type::D_); 621 | } 622 | template=E_)>::type* = nullptr> 623 | inline void registerCovarianceToPropertyHandler_(Eigen::MatrixXd& cov, PropertyHandler* mpPropertyHandler, const std::string& str, int j=0){} 624 | void createDefaultNames(){ 625 | createDefaultNames_("def"); 626 | } 627 | void createDefaultNames(const std::string& str){ 628 | createDefaultNames_(str); 629 | } 630 | template::type* = nullptr> 631 | inline void createDefaultNames_(const std::string& str){ 632 | std::get(mElements_).name_ = str + "_" + std::to_string(i); 633 | createDefaultNames_(str); 634 | } 635 | template=E_)>::type* = nullptr> 636 | inline void createDefaultNames_(const std::string& str){} 637 | template 638 | inline auto get(unsigned int j = 0) -> decltype (std::get(mElements_).get(j))& { 639 | return std::get(mElements_).get(j); 640 | }; 641 | template 642 | inline auto get(unsigned int j = 0) const -> decltype (std::get(mElements_).get(j))& { 643 | return std::get(mElements_).get(j); 644 | }; 645 | template::type* = nullptr> 646 | inline static constexpr unsigned int getId(unsigned int j = 0){ 647 | return D+j*std::tuple_element::type::E_; 648 | }; 649 | template0 & i::type* = nullptr> 650 | inline static constexpr unsigned int getId(unsigned int j = 0){ 651 | return getId::type::D_>(0)+j*std::tuple_element::type::E_; 652 | }; 653 | template::type* = nullptr> 654 | inline static unsigned int getElementId(unsigned int j){ 655 | if(j::type::D_){ 656 | return i; 657 | } else { 658 | return getElementId(j-std::tuple_element::type::D_); 659 | } 660 | }; 661 | template=E_)>::type* = nullptr> 662 | inline static unsigned int getElementId(unsigned int j){ 663 | std::cout << "ERROR: Exceeded state size" << std::endl; 664 | return i; 665 | }; 666 | template 667 | inline std::string& getName(){ 668 | return std::get(mElements_).name_; 669 | }; 670 | template 671 | inline const std::string& getName() const{ 672 | return std::get(mElements_).name_; 673 | }; 674 | inline std::string getName(unsigned int j) const{ 675 | return getName_(j); 676 | }; 677 | template::type* = nullptr> 678 | inline const std::string getName_(unsigned int j) const{ 679 | if(i==j){ 680 | return std::get(mElements_).name_; 681 | } else { 682 | return getName_(j); 683 | } 684 | }; 685 | template=E_)>::type* = nullptr> 686 | inline const std::string getName_(unsigned int j) const{ 687 | return "ERROR"; 688 | }; 689 | static State Identity(){ 690 | State identity; 691 | identity.setIdentity(); 692 | return identity; 693 | } 694 | }; 695 | 696 | template 697 | class TH_convert>: public State{ 698 | }; 699 | template 700 | class TH_convert>: public TH_multiple_elements{ 701 | }; 702 | 703 | } 704 | 705 | #endif /* LWF_STATE_HPP_ */ 706 | -------------------------------------------------------------------------------- /include/lightweight_filtering/Update.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Update.hpp 3 | * 4 | * Created on: Feb 9, 2014 5 | * Author: Bloeschm 6 | */ 7 | 8 | #ifndef LWF_UPDATEMODEL_HPP_ 9 | #define LWF_UPDATEMODEL_HPP_ 10 | 11 | #include "lightweight_filtering/common.hpp" 12 | #include "lightweight_filtering/ModelBase.hpp" 13 | #include "lightweight_filtering/PropertyHandler.hpp" 14 | #include "lightweight_filtering/SigmaPoints.hpp" 15 | #include "lightweight_filtering/OutlierDetection.hpp" 16 | #include 17 | #include 18 | 19 | namespace LWF{ 20 | 21 | template 22 | class Update: public ModelBase,Innovation,typename FilterState::mtState,Noise>, public PropertyHandler{ 23 | public: 24 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 25 | static_assert(!isCoupled || Noise::D_ == FilterState::noiseExtensionDim_,"Noise Size for coupled Update must match noise extension of prediction!"); 26 | typedef ModelBase,Innovation,typename FilterState::mtState,Noise> mtModelBase; 27 | typedef FilterState mtFilterState; 28 | typedef typename mtFilterState::mtState mtState; 29 | typedef typename mtModelBase::mtInputTuple mtInputTuple; 30 | typedef typename mtFilterState::mtPredictionMeas mtPredictionMeas; 31 | typedef typename mtFilterState::mtPredictionNoise mtPredictionNoise; 32 | typedef Innovation mtInnovation; 33 | typedef Meas mtMeas; 34 | typedef Noise mtNoise; 35 | typedef OutlierDetection mtOutlierDetection; 36 | mtMeas meas_; // TODO change to pointer, or remove 37 | static const bool coupledToPrediction_ = isCoupled; 38 | bool useSpecialLinearizationPoint_; 39 | bool useImprovedJacobian_; 40 | bool hasConverged_; 41 | bool successfulUpdate_; 42 | mutable bool cancelIteration_; 43 | mutable int candidateCounter_; 44 | mutable Eigen::MatrixXd H_; 45 | Eigen::MatrixXd Hlin_; 46 | Eigen::MatrixXd boxMinusJac_; 47 | Eigen::MatrixXd Hn_; 48 | Eigen::MatrixXd updnoiP_; 49 | Eigen::MatrixXd noiP_; 50 | Eigen::MatrixXd preupdnoiP_; 51 | Eigen::MatrixXd C_; 52 | mtInnovation y_; 53 | mutable Eigen::MatrixXd Py_; 54 | Eigen::MatrixXd Pyinv_; 55 | typename mtInnovation::mtDifVec innVector_; 56 | mtInnovation yIdentity_; 57 | typename mtState::mtDifVec updateVec_; 58 | mtState linState_; 59 | double updateVecNorm_; 60 | Eigen::MatrixXd K_; 61 | Eigen::MatrixXd Pyx_; 62 | mutable typename mtState::mtDifVec difVecLinInv_; 63 | 64 | SigmaPoints stateSigmaPoints_; 65 | SigmaPoints stateSigmaPointsNoi_; 66 | SigmaPoints innSigmaPoints_; 67 | SigmaPoints coupledStateSigmaPointsNoi_; 68 | SigmaPoints coupledInnSigmaPoints_; 69 | SigmaPoints,2*mtState::D_+1,2*mtState::D_+1,0> updateVecSP_; 70 | SigmaPoints posterior_; 71 | double alpha_; 72 | double beta_; 73 | double kappa_; 74 | double updateVecNormTermination_; 75 | int maxNumIteration_; 76 | int iterationNum_; 77 | mtOutlierDetection outlierDetection_; 78 | unsigned int numSequences; 79 | bool disablePreAndPostProcessingWarning_; 80 | Update(): H_((int)(mtInnovation::D_),(int)(mtState::D_)), 81 | Hlin_((int)(mtInnovation::D_),(int)(mtState::D_)), 82 | boxMinusJac_((int)(mtState::D_),(int)(mtState::D_)), 83 | Hn_((int)(mtInnovation::D_),(int)(mtNoise::D_)), 84 | updnoiP_((int)(mtNoise::D_),(int)(mtNoise::D_)), 85 | noiP_((int)(mtNoise::D_),(int)(mtNoise::D_)), 86 | preupdnoiP_((int)(mtPredictionNoise::D_),(int)(mtNoise::D_)), 87 | C_((int)(mtState::D_),(int)(mtInnovation::D_)), 88 | Py_((int)(mtInnovation::D_),(int)(mtInnovation::D_)), 89 | Pyinv_((int)(mtInnovation::D_),(int)(mtInnovation::D_)), 90 | K_((int)(mtState::D_),(int)(mtInnovation::D_)), 91 | Pyx_((int)(mtInnovation::D_),(int)(mtState::D_)){ 92 | alpha_ = 1e-3; 93 | beta_ = 2.0; 94 | kappa_ = 0.0; 95 | updateVecNormTermination_ = 1e-6; 96 | maxNumIteration_ = 10; 97 | updnoiP_.setIdentity(); 98 | updnoiP_ *= 0.0001; 99 | noiP_.setZero(); 100 | preupdnoiP_.setZero(); 101 | useSpecialLinearizationPoint_ = false; 102 | useImprovedJacobian_ = false; 103 | yIdentity_.setIdentity(); 104 | updateVec_.setIdentity(); 105 | refreshNoiseSigmaPoints(); 106 | refreshUKFParameter(); 107 | mtNoise n; 108 | n.setIdentity(); 109 | n.registerCovarianceToPropertyHandler_(updnoiP_,this,"UpdateNoise."); 110 | doubleRegister_.registerScalar("alpha",alpha_); 111 | doubleRegister_.registerScalar("beta",beta_); 112 | doubleRegister_.registerScalar("kappa",kappa_); 113 | doubleRegister_.registerScalar("updateVecNormTermination",updateVecNormTermination_); 114 | intRegister_.registerScalar("maxNumIteration",maxNumIteration_); 115 | outlierDetection_.setEnabledAll(false); 116 | numSequences = 1; 117 | disablePreAndPostProcessingWarning_ = false; 118 | }; 119 | virtual ~Update(){}; 120 | void refreshNoiseSigmaPoints(){ 121 | if(noiP_ != updnoiP_){ 122 | noiP_ = updnoiP_; 123 | stateSigmaPointsNoi_.computeFromZeroMeanGaussian(noiP_); 124 | } 125 | } 126 | void refreshUKFParameter(){ 127 | stateSigmaPoints_.computeParameter(alpha_,beta_,kappa_); 128 | innSigmaPoints_.computeParameter(alpha_,beta_,kappa_); 129 | coupledInnSigmaPoints_.computeParameter(alpha_,beta_,kappa_); 130 | updateVecSP_.computeParameter(alpha_,beta_,kappa_); 131 | posterior_.computeParameter(alpha_,beta_,kappa_); 132 | stateSigmaPointsNoi_.computeParameter(alpha_,beta_,kappa_); 133 | stateSigmaPointsNoi_.computeFromZeroMeanGaussian(noiP_); 134 | coupledStateSigmaPointsNoi_.computeParameter(alpha_,beta_,kappa_); 135 | } 136 | void refreshProperties(){ 137 | refreshPropertiesCustom(); 138 | refreshUKFParameter(); 139 | } 140 | virtual void refreshPropertiesCustom(){} 141 | void eval_(mtInnovation& x, const mtInputTuple& inputs, double dt) const{ 142 | evalInnovation(x,std::get<0>(inputs),std::get<1>(inputs)); 143 | } 144 | template::type* = nullptr> 145 | void jacInput_(Eigen::MatrixXd& F, const mtInputTuple& inputs, double dt) const{ 146 | jacState(F,std::get<0>(inputs)); 147 | } 148 | template::type* = nullptr> 149 | void jacInput_(Eigen::MatrixXd& F, const mtInputTuple& inputs, double dt) const{ 150 | jacNoise(F,std::get<0>(inputs)); 151 | } 152 | virtual void evalInnovation(mtInnovation& y, const mtState& state, const mtNoise& noise) const = 0; 153 | virtual void evalInnovationShort(mtInnovation& y, const mtState& state) const{ 154 | mtNoise n; // TODO get static for Identity() 155 | n.setIdentity(); 156 | evalInnovation(y,state,n); 157 | } 158 | virtual void jacState(Eigen::MatrixXd& F, const mtState& state) const = 0; 159 | virtual void jacNoise(Eigen::MatrixXd& F, const mtState& state) const = 0; 160 | virtual void preProcess(mtFilterState& filterState, const mtMeas& meas, bool& isFinished){ 161 | isFinished = false; 162 | if(!disablePreAndPostProcessingWarning_){ 163 | std::cout << "Warning: update preProcessing is not implemented!" << std::endl; 164 | } 165 | } 166 | virtual bool extraOutlierCheck(const mtState& state) const{ 167 | return hasConverged_; 168 | } 169 | virtual bool generateCandidates(const mtFilterState& filterState, mtState& candidate) const{ 170 | candidate = filterState.state_; 171 | candidateCounter_++; 172 | if(candidateCounter_<=1) 173 | return true; 174 | else 175 | return false; 176 | } 177 | virtual void postProcess(mtFilterState& filterState, const mtMeas& meas, const mtOutlierDetection& outlierDetection, bool& isFinished){ 178 | isFinished = true; 179 | if(!disablePreAndPostProcessingWarning_){ 180 | std::cout << "Warning: update postProcessing is not implemented!" << std::endl; 181 | } 182 | } 183 | int performUpdate(mtFilterState& filterState, const mtMeas& meas){ 184 | bool isFinished = true; 185 | int r = 0; 186 | do { 187 | preProcess(filterState,meas,isFinished); 188 | if(!isFinished){ 189 | switch(filterState.mode_){ 190 | case ModeEKF: 191 | r = performUpdateEKF(filterState,meas); 192 | break; 193 | case ModeUKF: 194 | r = performUpdateUKF(filterState,meas); 195 | break; 196 | case ModeIEKF: 197 | r = performUpdateIEKF(filterState,meas); 198 | break; 199 | default: 200 | r = performUpdateEKF(filterState,meas); 201 | break; 202 | } 203 | } 204 | postProcess(filterState,meas,outlierDetection_,isFinished); 205 | filterState.state_.fix(); 206 | enforceSymmetry(filterState.cov_); 207 | } while (!isFinished); 208 | return r; 209 | } 210 | int performUpdateEKF(mtFilterState& filterState, const mtMeas& meas){ 211 | meas_ = meas; 212 | if(!useSpecialLinearizationPoint_){ 213 | this->jacState(H_,filterState.state_); 214 | Hlin_ = H_; 215 | this->jacNoise(Hn_,filterState.state_); 216 | this->evalInnovationShort(y_,filterState.state_); 217 | } else { 218 | filterState.state_.boxPlus(filterState.difVecLin_,linState_); 219 | this->jacState(H_,linState_); 220 | if(useImprovedJacobian_){ 221 | filterState.state_.boxMinusJac(linState_,boxMinusJac_); 222 | Hlin_ = H_*boxMinusJac_; 223 | } else { 224 | Hlin_ = H_; 225 | } 226 | this->jacNoise(Hn_,linState_); 227 | this->evalInnovationShort(y_,linState_); 228 | } 229 | 230 | if(isCoupled){ 231 | C_ = filterState.G_*preupdnoiP_*Hn_.transpose(); 232 | Py_ = Hlin_*filterState.cov_*Hlin_.transpose() + Hn_*updnoiP_*Hn_.transpose() + Hlin_*C_ + C_.transpose()*Hlin_.transpose(); 233 | } else { 234 | Py_ = Hlin_*filterState.cov_*Hlin_.transpose() + Hn_*updnoiP_*Hn_.transpose(); 235 | } 236 | y_.boxMinus(yIdentity_,innVector_); 237 | 238 | // Outlier detection // TODO: adapt for special linearization point 239 | outlierDetection_.doOutlierDetection(innVector_,Py_,Hlin_); 240 | Pyinv_.setIdentity(); 241 | Py_.llt().solveInPlace(Pyinv_); 242 | 243 | // Kalman Update 244 | if(isCoupled){ 245 | K_ = (filterState.cov_*Hlin_.transpose()+C_)*Pyinv_; 246 | } else { 247 | K_ = filterState.cov_*Hlin_.transpose()*Pyinv_; 248 | } 249 | filterState.cov_ = filterState.cov_ - K_*Py_*K_.transpose(); 250 | if(!useSpecialLinearizationPoint_){ 251 | updateVec_ = -K_*innVector_; 252 | } else { 253 | filterState.state_.boxMinus(linState_,difVecLinInv_); 254 | updateVec_ = -K_*(innVector_+H_*difVecLinInv_); // includes correction for offseted linearization point, dif must be recomputed (a-b != (-(b-a))) 255 | } 256 | filterState.state_.boxPlus(updateVec_,filterState.state_); 257 | return 0; 258 | } 259 | int performUpdateIEKF(mtFilterState& filterState, const mtMeas& meas){ 260 | meas_ = meas; 261 | successfulUpdate_ = false; 262 | candidateCounter_ = 0; 263 | 264 | std::vector scores; 265 | std::vector> states; 266 | double bestScore = -1.0; 267 | mtState bestState; 268 | MXD bestCov; 269 | 270 | while(generateCandidates(filterState,linState_)){ 271 | cancelIteration_ = false; 272 | hasConverged_ = false; 273 | for(iterationNum_=0;iterationNum_jacState(H_,linState_); 275 | this->jacNoise(Hn_,linState_); 276 | this->evalInnovationShort(y_,linState_); 277 | 278 | if(isCoupled){ 279 | C_ = filterState.G_*preupdnoiP_*Hn_.transpose(); 280 | Py_ = H_*filterState.cov_*H_.transpose() + Hn_*updnoiP_*Hn_.transpose() + H_*C_ + C_.transpose()*H_.transpose(); 281 | } else { 282 | Py_ = H_*filterState.cov_*H_.transpose() + Hn_*updnoiP_*Hn_.transpose(); 283 | } 284 | y_.boxMinus(yIdentity_,innVector_); 285 | 286 | // Outlier detection 287 | outlierDetection_.doOutlierDetection(innVector_,Py_,H_); 288 | Pyinv_.setIdentity(); 289 | Py_.llt().solveInPlace(Pyinv_); 290 | 291 | // Kalman Update 292 | if(isCoupled){ 293 | K_ = (filterState.cov_*H_.transpose()+C_)*Pyinv_; 294 | } else { 295 | K_ = filterState.cov_*H_.transpose()*Pyinv_; 296 | } 297 | filterState.state_.boxMinus(linState_,difVecLinInv_); 298 | updateVec_ = -K_*(innVector_+H_*difVecLinInv_)+difVecLinInv_; // includes correction for offseted linearization point, dif must be recomputed (a-b != (-(b-a))) 299 | linState_.boxPlus(updateVec_,linState_); 300 | updateVecNorm_ = updateVec_.norm(); 301 | hasConverged_ = updateVecNorm_<=updateVecNormTermination_; 302 | } 303 | if(extraOutlierCheck(linState_)){ 304 | successfulUpdate_ = true; 305 | double score = (innVector_.transpose()*Pyinv_*innVector_)(0); 306 | scores.push_back(score); 307 | states.push_back(linState_); 308 | if(bestScore == -1.0 || score < bestScore){ 309 | bestScore = score; 310 | bestState = linState_; 311 | bestCov = filterState.cov_ - K_*Py_*K_.transpose(); 312 | } 313 | } 314 | } 315 | 316 | if(successfulUpdate_){ 317 | if(scores.size() == 1){ 318 | filterState.state_ = bestState; 319 | filterState.cov_ = bestCov; 320 | } else { 321 | bool foundOtherMin = false; 322 | for(auto it = states.begin();it!=states.end();it++){ 323 | bestState.boxMinus(*it,difVecLinInv_); 324 | if(difVecLinInv_.norm()>2*updateVecNormTermination_){ 325 | foundOtherMin = true; 326 | break; 327 | } 328 | } 329 | if(!foundOtherMin){ 330 | filterState.state_ = bestState; 331 | filterState.cov_ = bestCov; 332 | } else { 333 | successfulUpdate_ = false; 334 | } 335 | } 336 | } 337 | return 0; 338 | } 339 | int performUpdateUKF(mtFilterState& filterState, const mtMeas& meas){ 340 | meas_ = meas; 341 | handleUpdateSigmaPoints(filterState); 342 | y_.boxMinus(yIdentity_,innVector_); 343 | 344 | outlierDetection_.doOutlierDetection(innVector_,Py_,Pyx_); 345 | Pyinv_.setIdentity(); 346 | Py_.llt().solveInPlace(Pyinv_); 347 | 348 | // Kalman Update 349 | K_ = Pyx_.transpose()*Pyinv_; 350 | filterState.cov_ = filterState.cov_ - K_*Py_*K_.transpose(); 351 | updateVec_ = -K_*innVector_; 352 | 353 | // Adapt for proper linearization point 354 | updateVecSP_.computeFromZeroMeanGaussian(filterState.cov_); 355 | for(unsigned int i=0;i<2*mtState::D_+1;i++){ 356 | filterState.state_.boxPlus(updateVec_+updateVecSP_(i).v_,posterior_(i)); 357 | } 358 | posterior_.getMean(filterState.state_); 359 | posterior_.getCovarianceMatrix(filterState.state_,filterState.cov_); 360 | return 0; 361 | } 362 | template::type* = nullptr> 363 | void handleUpdateSigmaPoints(mtFilterState& filterState){ 364 | coupledStateSigmaPointsNoi_.extendZeroMeanGaussian(filterState.stateSigmaPointsNoi_,updnoiP_,preupdnoiP_); 365 | for(unsigned int i=0;ievalInnovation(coupledInnSigmaPoints_(i),filterState.stateSigmaPointsPre_(i),coupledStateSigmaPointsNoi_(i)); 367 | } 368 | coupledInnSigmaPoints_.getMean(y_); 369 | coupledInnSigmaPoints_.getCovarianceMatrix(y_,Py_); 370 | coupledInnSigmaPoints_.getCovarianceMatrix(filterState.stateSigmaPointsPre_,Pyx_); 371 | } 372 | template::type* = nullptr> 373 | void handleUpdateSigmaPoints(mtFilterState& filterState){ 374 | refreshNoiseSigmaPoints(); 375 | stateSigmaPoints_.computeFromGaussian(filterState.state_,filterState.cov_); 376 | for(unsigned int i=0;ievalInnovation(innSigmaPoints_(i),stateSigmaPoints_(i),stateSigmaPointsNoi_(i)); 378 | } 379 | innSigmaPoints_.getMean(y_); 380 | innSigmaPoints_.getCovarianceMatrix(y_,Py_); 381 | innSigmaPoints_.getCovarianceMatrix(stateSigmaPoints_,Pyx_); 382 | } 383 | bool testUpdateJacs(double d = 1e-6,double th = 1e-6){ 384 | mtState state; 385 | mtMeas meas; 386 | unsigned int s = 1; 387 | state.setRandom(s); 388 | meas.setRandom(s); 389 | return testUpdateJacs(state,meas,d,th); 390 | } 391 | bool testUpdateJacs(const mtState& state, const mtMeas& meas, double d = 1e-6,double th = 1e-6){ 392 | mtInputTuple inputs; 393 | const double dt = 1.0; 394 | std::get<0>(inputs) = state; 395 | std::get<1>(inputs).setIdentity(); // Noise is always set to zero for Jacobians 396 | meas_ = meas; 397 | return this->testJacs(inputs,d,th,dt); 398 | } 399 | }; 400 | 401 | } 402 | 403 | #endif /* LWF_UPDATEMODEL_HPP_ */ 404 | -------------------------------------------------------------------------------- /include/lightweight_filtering/common.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Common.hpp 3 | * 4 | * Created on: Feb 9, 2014 5 | * Author: Bloeschm 6 | */ 7 | 8 | #ifndef LWF_COMMON_HPP_ 9 | #define LWF_COMMON_HPP_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "kindr/Core" 17 | #include "lightweight_filtering/PropertyHandler.hpp" 18 | 19 | typedef kindr::RotationQuaternionPD QPD; 20 | typedef kindr::RotationMatrixPD MPD; 21 | typedef Eigen::Vector3d V3D; 22 | typedef Eigen::Matrix3d M3D; 23 | typedef Eigen::VectorXd VXD; 24 | typedef Eigen::MatrixXd MXD; 25 | inline M3D gSM(const V3D& vec){ 26 | return kindr::getSkewMatrixFromVector(vec); 27 | } 28 | 29 | static void enforceSymmetry(MXD& mat){ 30 | mat = 0.5*(mat+mat.transpose()).eval(); 31 | } 32 | 33 | inline M3D Lmat (const V3D& a) { 34 | return kindr::getJacobianOfExponentialMap(a); 35 | } 36 | 37 | namespace LWF{ 38 | enum FilteringMode{ 39 | ModeEKF, 40 | ModeUKF, 41 | ModeIEKF 42 | }; 43 | } 44 | 45 | #endif /* LWF_COMMON_HPP_ */ 46 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lightweight_filtering 4 | Lightweight filtering library 5 | https://bitbucket.org/ethz-asl-lr/lightweight_filtering 6 | Michael Bloesch 7 | Michael Bloesch 8 | BSD 9 | 0.0.9 10 | catkin 11 | kindr 12 | 13 | -------------------------------------------------------------------------------- /src/testFilterBase.cpp: -------------------------------------------------------------------------------- 1 | #include "lightweight_filtering/TestClasses.hpp" 2 | #include "lightweight_filtering/FilterBase.hpp" 3 | #include "lightweight_filtering/common.hpp" 4 | #include "gtest/gtest.h" 5 | #include 6 | 7 | using namespace LWFTest; 8 | 9 | typedef ::testing::Types< 10 | NonlinearTest, 11 | LinearTest 12 | > TestClasses; 13 | 14 | // The fixture for testing class MeasurementTimeline 15 | class MeasurementTimelineTest : public ::testing::Test { 16 | protected: 17 | MeasurementTimelineTest() { 18 | for(unsigned int i=0;i timeline_; 27 | static const unsigned int N_ = 5; 28 | double times_[N_]; 29 | double values_[N_]; 30 | }; 31 | 32 | // Test constructors 33 | TEST_F(MeasurementTimelineTest, constructors) { 34 | LWF::MeasurementTimeline timeline; 35 | ASSERT_EQ(timeline.maxWaitTime_,0.1); 36 | } 37 | 38 | // Test addMeas 39 | TEST_F(MeasurementTimelineTest, addMeas) { 40 | for(int i=N_-1;i>=0;i--){ // Reverse for detecting FrontWarning 41 | timeline_.addMeas(values_[i],times_[i]); 42 | for(int j=N_-1;j>=i;j--){ 43 | ASSERT_EQ(timeline_.measMap_.at(times_[j]),values_[j]); 44 | } 45 | } 46 | } 47 | 48 | // Test clean 49 | TEST_F(MeasurementTimelineTest, clean) { 50 | for(unsigned int i=0;i 129 | class FilterBaseTest : public ::testing::Test, public TestClass { 130 | protected: 131 | FilterBaseTest() { 132 | this->init(this->testFilterState_.state_,this->testUpdateMeas_,this->testPredictionMeas_); 133 | this->testFilter_.predictionTimeline_.maxWaitTime_ = 1.0; 134 | this->testFilter2_.predictionTimeline_.maxWaitTime_ = 1.0; 135 | std::get<0>(this->testFilter_.updateTimelineTuple_).maxWaitTime_ = 1.0; 136 | std::get<0>(this->testFilter2_.updateTimelineTuple_).maxWaitTime_ = 1.0; 137 | std::get<1>(this->testFilter_.updateTimelineTuple_).maxWaitTime_ = 0.0; 138 | std::get<1>(this->testFilter2_.updateTimelineTuple_).maxWaitTime_ = 0.0; 139 | switch(id_){ 140 | case 0: 141 | this->testFilter_.readFromInfo("test_nonlinear.info"); 142 | this->testFilter2_.readFromInfo("test_nonlinear.info"); 143 | break; 144 | case 1: 145 | this->testFilter_.readFromInfo("test_linear.info"); 146 | this->testFilter2_.readFromInfo("test_linear.info"); 147 | break; 148 | default: 149 | this->testFilter_.readFromInfo("test_nonlinear.info"); 150 | this->testFilter2_.readFromInfo("test_nonlinear.info"); 151 | }; 152 | this->testFilter_.reset(); 153 | this->testFilter2_.reset(); 154 | } 155 | virtual ~FilterBaseTest() { 156 | } 157 | using typename TestClass::mtState; 158 | using typename TestClass::mtFilterState; 159 | using typename TestClass::mtUpdateMeas; 160 | using typename TestClass::mtUpdateNoise; 161 | using typename TestClass::mtInnovation; 162 | using typename TestClass::mtPredictionNoise; 163 | using typename TestClass::mtPredictionMeas; 164 | using typename TestClass::mtUpdateExample; 165 | using typename TestClass::mtPredictionExample; 166 | using typename TestClass::mtPredictAndUpdateExample; 167 | using TestClass::id_; 168 | LWF::FilterBase testFilter_; 169 | LWF::FilterBase testFilter2_; 170 | mtFilterState testFilterState_; 171 | typename TestClass::mtState::mtDifVec difVec_; 172 | mtUpdateMeas testUpdateMeas_; 173 | mtPredictionMeas testPredictionMeas_; 174 | const double dt_ = 0.1; 175 | }; 176 | 177 | TYPED_TEST_CASE(FilterBaseTest, TestClasses); 178 | 179 | // Test constructors 180 | TYPED_TEST(FilterBaseTest, constructors) { 181 | LWF::FilterBase testFilter; 182 | } 183 | 184 | // Test propertyHandler 185 | TYPED_TEST(FilterBaseTest, propertyHandler) { 186 | // Generate parameters 187 | MXD prenoiP((int)TestFixture::mtPredictionExample::mtNoise::D_,(int)TestFixture::mtPredictionExample::mtNoise::D_); 188 | prenoiP.setZero(); 189 | for(unsigned int i=0;i preupdnoiP; 203 | preupdnoiP.setZero(); 204 | for(unsigned int i=0;itestFilter_.mPrediction_.prenoiP_ = prenoiP; 218 | std::get<0>(this->testFilter_.mUpdates_).updnoiP_ = updnoiP; 219 | std::get<1>(this->testFilter_.mUpdates_).updnoiP_ = updnoiP2; 220 | std::get<1>(this->testFilter_.mUpdates_).preupdnoiP_ = preupdnoiP; 221 | this->testFilter_.init_.cov_ = initP; 222 | this->testFilter_.init_.state_ = initState; 223 | 224 | // Write to file 225 | this->testFilter_.writeToInfo("test.info"); 226 | 227 | // Set parameters zero 228 | this->testFilter_.mPrediction_.prenoiP_.setZero(); 229 | std::get<0>(this->testFilter_.mUpdates_).updnoiP_.setZero(); 230 | std::get<1>(this->testFilter_.mUpdates_).updnoiP_.setZero(); 231 | std::get<1>(this->testFilter_.mUpdates_).preupdnoiP_.setZero(); 232 | this->testFilter_.init_.cov_.setZero(); 233 | this->testFilter_.init_.state_.setIdentity(); 234 | 235 | // Read parameters from file and compare 236 | this->testFilter_.readFromInfo("test.info"); 237 | ASSERT_NEAR((this->testFilter_.mPrediction_.prenoiP_-prenoiP).norm(),0.0,1e-6); 238 | ASSERT_NEAR((std::get<0>(this->testFilter_.mUpdates_).updnoiP_-updnoiP).norm(),0.0,1e-6); 239 | ASSERT_NEAR((std::get<1>(this->testFilter_.mUpdates_).updnoiP_-updnoiP2).norm(),0.0,1e-6); 240 | // ASSERT_NEAR((std::get<1>(this->testFilter_.mUpdates_).preupdnoiP_-preupdnoiP).norm(),0.0,1e-6); 241 | ASSERT_NEAR((this->testFilter_.init_.cov_-initP).norm(),0.0,1e-6); 242 | typename TestFixture::mtState::mtDifVec difVec; 243 | this->testFilter_.init_.state_.boxMinus(initState,difVec); 244 | ASSERT_NEAR(difVec.norm(),0.0,1e-6); 245 | } 246 | 247 | // Test updateSafe (Only for 1 update type (wait time set to zero for the other)), co-test getSafeTime() and setSafeWarningTime() and clean() 248 | TYPED_TEST(FilterBaseTest, updateSafe) { 249 | double safeTime = 0.0; 250 | this->testFilter_.safeWarningTime_ = 0.1; 251 | 252 | std::cout << "Should print warning (2):" << std::endl; 253 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.1); 254 | this->testFilter_.template addUpdateMeas<0>(this->testUpdateMeas_,0.1); 255 | ASSERT_TRUE(this->testFilter_.getSafeTime(safeTime)); 256 | ASSERT_EQ(safeTime,0.1); 257 | this->testFilter_.updateSafe(); 258 | ASSERT_EQ(this->testFilter_.safe_.t_,0.1); 259 | ASSERT_EQ(this->testFilter_.predictionTimeline_.measMap_.size(),1); 260 | ASSERT_EQ(std::get<0>(this->testFilter_.updateTimelineTuple_).measMap_.size(),1); 261 | 262 | std::cout << "Should print warning (2):" << std::endl; 263 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.1); 264 | this->testFilter_.template addUpdateMeas<0>(this->testUpdateMeas_,0.1); 265 | ASSERT_TRUE(!this->testFilter_.getSafeTime(safeTime)); 266 | ASSERT_EQ(safeTime,0.1); 267 | this->testFilter_.updateSafe(); 268 | ASSERT_EQ(this->testFilter_.safe_.t_,0.1); 269 | ASSERT_EQ(this->testFilter_.predictionTimeline_.measMap_.size(),1); 270 | ASSERT_EQ(std::get<0>(this->testFilter_.updateTimelineTuple_).measMap_.size(),1); 271 | 272 | this->testFilter_.template addUpdateMeas<0>(this->testUpdateMeas_,0.2); 273 | ASSERT_TRUE(!this->testFilter_.getSafeTime(safeTime)); 274 | ASSERT_EQ(safeTime,0.1); 275 | this->testFilter_.updateSafe(); 276 | ASSERT_EQ(this->testFilter_.safe_.t_,0.1); 277 | ASSERT_EQ(this->testFilter_.predictionTimeline_.measMap_.size(),1); 278 | ASSERT_EQ(std::get<0>(this->testFilter_.updateTimelineTuple_).measMap_.size(),2); 279 | 280 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.2); 281 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.3); 282 | ASSERT_TRUE(this->testFilter_.getSafeTime(safeTime)); 283 | ASSERT_EQ(safeTime,0.2); 284 | this->testFilter_.updateSafe(); 285 | ASSERT_EQ(this->testFilter_.safe_.t_,0.2); 286 | ASSERT_EQ(this->testFilter_.predictionTimeline_.measMap_.size(),1); 287 | ASSERT_EQ(std::get<0>(this->testFilter_.updateTimelineTuple_).measMap_.size(),1); 288 | 289 | this->testFilter_.template addUpdateMeas<0>(this->testUpdateMeas_,0.3); 290 | ASSERT_TRUE(this->testFilter_.getSafeTime(safeTime)); 291 | ASSERT_EQ(safeTime,0.3); 292 | this->testFilter_.updateSafe(); 293 | ASSERT_EQ(this->testFilter_.safe_.t_,0.3); 294 | ASSERT_EQ(this->testFilter_.predictionTimeline_.measMap_.size(),1); 295 | ASSERT_EQ(std::get<0>(this->testFilter_.updateTimelineTuple_).measMap_.size(),1); 296 | } 297 | 298 | // Test updateFront 299 | TYPED_TEST(FilterBaseTest, updateFront) { 300 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.1); 301 | this->testFilter_.template addUpdateMeas<0>(this->testUpdateMeas_,0.1); 302 | ASSERT_TRUE(this->testFilter_.gotFrontWarning_==false); 303 | this->testFilter_.updateFront(0.5); 304 | ASSERT_TRUE(this->testFilter_.gotFrontWarning_==false); 305 | ASSERT_EQ(this->testFilter_.safe_.t_,0.1); 306 | ASSERT_EQ(this->testFilter_.front_.t_,0.5); 307 | 308 | this->testFilter_.template addUpdateMeas<0>(this->testUpdateMeas_,0.2); 309 | ASSERT_TRUE(this->testFilter_.gotFrontWarning_==true); 310 | this->testFilter_.updateFront(0.2); 311 | ASSERT_TRUE(this->testFilter_.gotFrontWarning_==false); 312 | ASSERT_EQ(this->testFilter_.safe_.t_,0.1); 313 | ASSERT_EQ(this->testFilter_.front_.t_,0.2); 314 | 315 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.2); 316 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.3); 317 | ASSERT_TRUE(this->testFilter_.gotFrontWarning_==true); 318 | this->testFilter_.updateFront(0.3); 319 | ASSERT_TRUE(this->testFilter_.gotFrontWarning_==false); 320 | ASSERT_EQ(this->testFilter_.safe_.t_,0.2); 321 | ASSERT_EQ(this->testFilter_.front_.t_,0.3); 322 | 323 | this->testFilter_.template addUpdateMeas<0>(this->testUpdateMeas_,0.3); 324 | ASSERT_TRUE(this->testFilter_.gotFrontWarning_==true); 325 | this->testFilter_.updateFront(0.3); 326 | ASSERT_TRUE(this->testFilter_.gotFrontWarning_==false); 327 | ASSERT_EQ(this->testFilter_.safe_.t_,0.3); 328 | ASSERT_EQ(this->testFilter_.front_.t_,0.3); 329 | } 330 | 331 | // Test high level logic 332 | TYPED_TEST(FilterBaseTest, highlevel) { 333 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.1); 334 | this->testFilter_.template addUpdateMeas<0>(this->testUpdateMeas_,0.1); 335 | this->testFilter_.updateSafe(); 336 | this->testFilter2_.addPredictionMeas(this->testPredictionMeas_,0.1); 337 | this->testFilter2_.template addUpdateMeas<0>(this->testUpdateMeas_,0.1); 338 | this->testFilter2_.updateSafe(); 339 | 340 | this->testFilter2_.safe_.state_.boxMinus(this->testFilter_.safe_.state_,this->difVec_); 341 | ASSERT_EQ(this->testFilter_.safe_.t_,this->testFilter2_.safe_.t_); 342 | ASSERT_NEAR(this->difVec_.norm(),0.0,1e-6); 343 | ASSERT_NEAR((this->testFilter2_.safe_.cov_-this->testFilter_.safe_.cov_).norm(),0.0,1e-6); 344 | 345 | 346 | this->testFilter_.template addUpdateMeas<0>(this->testUpdateMeas_,0.2); 347 | this->testFilter_.updateFront(0.2); 348 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.2); 349 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.3); 350 | this->testFilter_.updateFront(0.3); 351 | this->testFilter_.template addUpdateMeas<0>(this->testUpdateMeas_,0.3); 352 | this->testFilter_.updateFront(0.3); 353 | 354 | this->testFilter2_.template addUpdateMeas<0>(this->testUpdateMeas_,0.2); 355 | this->testFilter2_.addPredictionMeas(this->testPredictionMeas_,0.2); 356 | this->testFilter2_.addPredictionMeas(this->testPredictionMeas_,0.3); 357 | this->testFilter2_.template addUpdateMeas<0>(this->testUpdateMeas_,0.3); 358 | this->testFilter2_.updateSafe(); 359 | 360 | this->testFilter2_.safe_.state_.boxMinus(this->testFilter_.safe_.state_,this->difVec_); 361 | ASSERT_EQ(this->testFilter_.safe_.t_,this->testFilter2_.safe_.t_); 362 | ASSERT_NEAR(this->difVec_.norm(),0.0,1e-6); 363 | ASSERT_NEAR((this->testFilter2_.safe_.cov_-this->testFilter_.safe_.cov_).norm(),0.0,1e-6); 364 | } 365 | 366 | // Test high level logic 2: coupled 367 | TYPED_TEST(FilterBaseTest, highlevel2) { 368 | std::get<1>(this->testFilter_.mUpdates_).preupdnoiP_.block(0,0,3,3) = M3D::Identity()*0.00009; 369 | std::get<1>(this->testFilter2_.mUpdates_).preupdnoiP_.block(0,0,3,3) = M3D::Identity()*0.00009; 370 | this->testFilterState_.state_ = this->testFilter_.safe_.state_; 371 | this->testFilterState_.cov_ = this->testFilter_.safe_.cov_; 372 | std::get<1>(this->testFilter_.updateTimelineTuple_).maxWaitTime_ = 1.0; 373 | std::get<1>(this->testFilter2_.updateTimelineTuple_).maxWaitTime_ = 1.0; 374 | std::get<0>(this->testFilter_.updateTimelineTuple_).maxWaitTime_ = 0.0; 375 | std::get<0>(this->testFilter2_.updateTimelineTuple_).maxWaitTime_ = 0.0; 376 | // TestFilter 377 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.1); 378 | this->testFilter_.template addUpdateMeas<1>(this->testUpdateMeas_,0.1); 379 | this->testFilter_.updateSafe(); 380 | // TestFilter2 381 | this->testFilter2_.addPredictionMeas(this->testPredictionMeas_,0.1); 382 | this->testFilter2_.template addUpdateMeas<1>(this->testUpdateMeas_,0.1); 383 | this->testFilter2_.updateSafe(); 384 | // Direct 385 | this->testFilter_.mPrediction_.performPredictionEKF(this->testFilterState_,this->testPredictionMeas_,0.1); 386 | std::get<1>(this->testFilter_.mUpdates_).performUpdateEKF(this->testFilterState_,this->testUpdateMeas_); 387 | 388 | // Compare 389 | this->testFilter2_.safe_.state_.boxMinus(this->testFilter_.safe_.state_,this->difVec_); 390 | ASSERT_EQ(this->testFilter_.safe_.t_,this->testFilter2_.safe_.t_); 391 | ASSERT_NEAR(this->difVec_.norm(),0.0,1e-6); 392 | ASSERT_NEAR((this->testFilter2_.safe_.cov_-this->testFilter_.safe_.cov_).norm(),0.0,1e-6); 393 | this->testFilter_.safe_.state_.boxMinus(this->testFilterState_.state_,this->difVec_); 394 | ASSERT_NEAR(this->difVec_.norm(),0.0,1e-6); 395 | ASSERT_NEAR((this->testFilter_.safe_.cov_-this->testFilterState_.cov_).norm(),0.0,1e-6); 396 | 397 | // TestFilter 398 | this->testFilter_.template addUpdateMeas<1>(this->testUpdateMeas_,0.2); 399 | this->testFilter_.updateFront(0.2); 400 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.2); 401 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.3); 402 | this->testFilter_.updateFront(0.3); 403 | this->testFilter_.template addUpdateMeas<1>(this->testUpdateMeas_,0.3); 404 | this->testFilter_.updateFront(0.3); 405 | // TestFilter2 406 | this->testFilter2_.template addUpdateMeas<1>(this->testUpdateMeas_,0.2); 407 | this->testFilter2_.addPredictionMeas(this->testPredictionMeas_,0.2); 408 | this->testFilter2_.addPredictionMeas(this->testPredictionMeas_,0.3); 409 | this->testFilter2_.template addUpdateMeas<1>(this->testUpdateMeas_,0.3); 410 | this->testFilter2_.updateSafe(); 411 | // Direct 412 | this->testFilter_.mPrediction_.performPredictionEKF(this->testFilterState_,this->testPredictionMeas_,0.1); 413 | std::get<1>(this->testFilter_.mUpdates_).performUpdateEKF(this->testFilterState_,this->testUpdateMeas_); 414 | this->testFilter_.mPrediction_.performPredictionEKF(this->testFilterState_,this->testPredictionMeas_,0.1); 415 | std::get<1>(this->testFilter_.mUpdates_).performUpdateEKF(this->testFilterState_,this->testUpdateMeas_); 416 | 417 | // Compare 418 | this->testFilter2_.safe_.state_.boxMinus(this->testFilter_.safe_.state_,this->difVec_); 419 | ASSERT_EQ(this->testFilter_.safe_.t_,this->testFilter2_.safe_.t_); 420 | ASSERT_NEAR(this->difVec_.norm(),0.0,1e-6); 421 | ASSERT_NEAR((this->testFilter2_.safe_.cov_-this->testFilter_.safe_.cov_).norm(),0.0,1e-6); 422 | this->testFilter_.safe_.state_.boxMinus(this->testFilterState_.state_,this->difVec_); 423 | ASSERT_NEAR(this->difVec_.norm(),0.0,1e-6); 424 | ASSERT_NEAR((this->testFilter_.safe_.cov_-this->testFilterState_.cov_).norm(),0.0,1e-6); 425 | } 426 | 427 | // Test high level logic 3: merged 428 | TYPED_TEST(FilterBaseTest, highlevel3) { 429 | this->testFilterState_.state_ = this->testFilter2_.safe_.state_; 430 | this->testFilterState_.cov_ = this->testFilter2_.safe_.cov_; 431 | // TestFilter and direct method 432 | this->testFilterState_.usePredictionMerge_ = true; 433 | this->testFilter_.safe_.usePredictionMerge_ = true; 434 | this->testFilter2_.safe_.usePredictionMerge_ = true; 435 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.1); 436 | this->testFilter_.template addUpdateMeas<0>(this->testUpdateMeas_,0.1); 437 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.2); 438 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.3); 439 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.4); 440 | this->testFilter_.addPredictionMeas(this->testPredictionMeas_,0.5); 441 | this->testFilter_.template addUpdateMeas<0>(this->testUpdateMeas_,0.5); 442 | this->testFilter_.mPrediction_.performPredictionEKF(this->testFilterState_,this->testPredictionMeas_,0.1); 443 | std::get<0>(this->testFilter_.mUpdates_).performUpdateEKF(this->testFilterState_,this->testUpdateMeas_); 444 | this->testFilter_.mPrediction_.predictMergedEKF(this->testFilterState_,0.5,this->testFilter_.predictionTimeline_.measMap_); 445 | std::get<0>(this->testFilter_.mUpdates_).performUpdateEKF(this->testFilterState_,this->testUpdateMeas_); 446 | this->testFilter_.updateSafe(); 447 | // TestFilter2 448 | this->testFilter2_.addPredictionMeas(this->testPredictionMeas_,0.1); 449 | this->testFilter2_.template addUpdateMeas<0>(this->testUpdateMeas_,0.1); 450 | this->testFilter2_.updateSafe(); 451 | this->testFilter2_.addPredictionMeas(this->testPredictionMeas_,0.2); 452 | this->testFilter2_.updateSafe(); 453 | this->testFilter2_.addPredictionMeas(this->testPredictionMeas_,0.3); 454 | this->testFilter2_.updateSafe(); 455 | this->testFilter2_.addPredictionMeas(this->testPredictionMeas_,0.4); 456 | this->testFilter2_.updateSafe(); 457 | this->testFilter2_.addPredictionMeas(this->testPredictionMeas_,0.5); 458 | this->testFilter2_.updateSafe(); 459 | this->testFilter2_.template addUpdateMeas<0>(this->testUpdateMeas_,0.5); 460 | this->testFilter2_.updateSafe(); 461 | 462 | // Compare 463 | this->testFilter2_.safe_.state_.boxMinus(this->testFilter_.safe_.state_,this->difVec_); 464 | ASSERT_EQ(this->testFilter_.safe_.t_,this->testFilter2_.safe_.t_); 465 | ASSERT_NEAR(this->difVec_.norm(),0.0,1e-6); 466 | ASSERT_NEAR((this->testFilter2_.safe_.cov_-this->testFilter_.safe_.cov_).norm(),0.0,1e-6); 467 | this->testFilter_.safe_.state_.boxMinus(this->testFilterState_.state_,this->difVec_); 468 | ASSERT_NEAR(this->difVec_.norm(),0.0,1e-6); 469 | ASSERT_NEAR((this->testFilter_.safe_.cov_-this->testFilterState_.cov_).norm(),0.0,1e-6); 470 | } 471 | 472 | int main(int argc, char **argv) { 473 | ::testing::InitGoogleTest(&argc, argv); 474 | return RUN_ALL_TESTS(); 475 | } 476 | -------------------------------------------------------------------------------- /src/testGIFPrediction.cpp: -------------------------------------------------------------------------------- 1 | #include "lightweight_filtering/TestClasses.hpp" 2 | #include "lightweight_filtering/common.hpp" 3 | #include "gtest/gtest.h" 4 | #include 5 | 6 | using namespace LWFTest; 7 | 8 | typedef ::testing::Types< 9 | NonlinearTest, 10 | LinearTest 11 | > TestClasses; 12 | 13 | // The fixture for testing class PredictionModel 14 | template 15 | class GIFPredictionModelTest : public ::testing::Test, public TestClass { 16 | protected: 17 | GIFPredictionModelTest() { 18 | unsigned int s = 1; 19 | testState_.setRandom(s); 20 | testPredictionMeas_.setRandom(s); 21 | updateMeas_.setRandom(s); 22 | TestClass::mergePredictionAndUpdateMeas(GIFMeasWithUpdate_,testPredictionMeas_,updateMeas_); 23 | predictionExample_.prenoiP_.setIdentity(); 24 | predictionExample_.prenoiP_ = 0.01*predictionExample_.prenoiP_; 25 | predictionExample_.prenoiP_.block(0,0,mtState::D_,mtState::D_) = 0.25*predictionExample_.prenoiP_.block(0,0,mtState::D_,mtState::D_); 26 | updateExample_.updnoiP_.setIdentity(); 27 | updateExample_.updnoiP_ = 0.04*updateExample_.updnoiP_; 28 | GIFpredictionExample_.noiP_ = predictionExample_.prenoiP_; 29 | GIFpredictionExampleWithUpdate_.noiP_.block(0,0,mtPredictionNoise::D_,mtPredictionNoise::D_) = predictionExample_.prenoiP_; 30 | GIFpredictionExampleWithUpdate_.noiP_.block(mtPredictionNoise::D_,mtPredictionNoise::D_,mtUpdateNoise::D_,mtUpdateNoise::D_) = updateExample_.updnoiP_; 31 | } 32 | virtual ~GIFPredictionModelTest() { 33 | } 34 | using typename TestClass::mtState; 35 | using typename TestClass::mtFilterState; 36 | using typename TestClass::mtUpdateMeas; 37 | using typename TestClass::mtUpdateNoise; 38 | using typename TestClass::mtInnovation; 39 | using typename TestClass::mtPredictionNoise; 40 | using typename TestClass::mtPredictionMeas; 41 | using typename TestClass::mtUpdateExample; 42 | using typename TestClass::mtPredictionExample; 43 | using typename TestClass::mtPredictAndUpdateExample; 44 | using typename TestClass::mtGIFPredictionExample; 45 | using typename TestClass::mtGIFPredictionExampleWithUpdate; 46 | using typename TestClass::mtGIFMeasWithUpdate; 47 | mtGIFPredictionExample GIFpredictionExample_; 48 | mtPredictionExample predictionExample_; 49 | mtState testState_; 50 | mtPredictionMeas testPredictionMeas_; 51 | mtGIFPredictionExampleWithUpdate GIFpredictionExampleWithUpdate_; 52 | mtGIFMeasWithUpdate GIFMeasWithUpdate_; 53 | mtUpdateMeas updateMeas_; 54 | mtUpdateExample updateExample_; 55 | const double dt_ = 0.1; 56 | // std::map measMap_; 57 | }; 58 | 59 | TYPED_TEST_CASE(GIFPredictionModelTest, TestClasses); 60 | 61 | // Test constructors and Jacobians 62 | TYPED_TEST(GIFPredictionModelTest, constructors) { 63 | typename TestFixture::mtFilterState filterState; 64 | double dt = 0.1; 65 | this->GIFpredictionExample_.performPrediction(filterState,this->testPredictionMeas_,dt); 66 | typename TestFixture::mtGIFPredictionExample testPrediction; 67 | ASSERT_EQ((testPrediction.noiP_-Eigen::Matrix::Identity()*0.0001).norm(),0.0); 68 | ASSERT_TRUE(this->GIFpredictionExample_.testPredictionJacs(1e-8,1e-5,0.1)); 69 | ASSERT_TRUE(this->GIFpredictionExampleWithUpdate_.testPredictionJacs(1e-8,1e-5,0.1)); 70 | } 71 | 72 | // Test comparePredict 73 | TYPED_TEST(GIFPredictionModelTest, comparePredict) { 74 | typename TestFixture::mtFilterState filterState1; 75 | typename TestFixture::mtFilterState filterState2; 76 | filterState1.cov_.setIdentity(); 77 | filterState1.cov_ = filterState1.cov_*0.01; 78 | filterState2.cov_ = filterState1.cov_.inverse(); 79 | filterState1.state_ = this->testState_; 80 | filterState2.state_ = this->testState_; 81 | this->predictionExample_.performPredictionEKF(filterState1,this->testPredictionMeas_,this->dt_); 82 | this->GIFpredictionExample_.performPrediction(filterState2,this->testPredictionMeas_,this->dt_); 83 | typename TestFixture::mtState::mtDifVec dif; 84 | filterState1.state_.boxMinus(filterState2.state_,dif); 85 | switch(TestFixture::id_){ 86 | case 0: 87 | ASSERT_NEAR(dif.norm(),0.0,1e-5); 88 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_.inverse()).norm(),0.0,1e-6); 89 | break; 90 | case 1: 91 | ASSERT_NEAR(dif.norm(),0.0,1e-9); 92 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_.inverse()).norm(),0.0,1e-10); 93 | break; 94 | default: 95 | ASSERT_NEAR(dif.norm(),0.0,1e-5); 96 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_.inverse()).norm(),0.0,1e-6); 97 | }; 98 | } 99 | 100 | // Test comparePredictWithUpdate 101 | TYPED_TEST(GIFPredictionModelTest, comparePredictWithUpdate) { 102 | typename TestFixture::mtFilterState filterState1; 103 | typename TestFixture::mtFilterState filterState2; 104 | filterState1.cov_.setIdentity(); 105 | filterState1.cov_ = filterState1.cov_*0.01; 106 | filterState2.cov_ = filterState1.cov_.inverse(); 107 | filterState1.state_ = this->testState_; 108 | filterState2.state_ = this->testState_; 109 | this->predictionExample_.performPredictionEKF(filterState1,this->testPredictionMeas_,this->dt_); 110 | this->updateExample_.performUpdateEKF(filterState1,this->updateMeas_); 111 | this->GIFpredictionExampleWithUpdate_.performPrediction(filterState2,this->GIFMeasWithUpdate_,this->dt_); 112 | typename TestFixture::mtState::mtDifVec dif; 113 | filterState1.state_.boxMinus(filterState2.state_,dif); 114 | switch(TestFixture::id_){ 115 | case 0: 116 | ASSERT_NEAR(dif.norm(),0.0,1e-5); 117 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_.inverse()).norm(),0.0,1e-6); 118 | break; 119 | case 1: 120 | ASSERT_NEAR(dif.norm(),0.0,1e-9); 121 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_.inverse()).norm(),0.0,1e-10); 122 | break; 123 | default: 124 | ASSERT_NEAR(dif.norm(),0.0,1e-5); 125 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_.inverse()).norm(),0.0,1e-6); 126 | }; 127 | } 128 | 129 | int main(int argc, char **argv) { 130 | ::testing::InitGoogleTest(&argc, argv); 131 | return RUN_ALL_TESTS(); 132 | } 133 | -------------------------------------------------------------------------------- /src/testModelBase.cpp: -------------------------------------------------------------------------------- 1 | #include "lightweight_filtering/State.hpp" 2 | #include "lightweight_filtering/ModelBase.hpp" 3 | #include "lightweight_filtering/common.hpp" 4 | #include "gtest/gtest.h" 5 | #include 6 | 7 | class Input: public LWF::State,2>,LWF::TH_multiple_elements>{ 8 | public: 9 | enum StateNames{ 10 | v0, v1, q0, q1 11 | }; 12 | Input(){}; 13 | ~Input(){}; 14 | }; 15 | class Output: public LWF::State,LWF::QuaternionElement>{ 16 | public: 17 | enum StateNames{ 18 | v0, q0 19 | }; 20 | Output(){}; 21 | ~Output(){}; 22 | }; 23 | class Meas: public LWF::State,LWF::QuaternionElement>{ 24 | public: 25 | enum StateNames{ 26 | v0, q0 27 | }; 28 | Meas(){}; 29 | ~Meas(){}; 30 | }; 31 | class Noise: public LWF::State,2>>{ 32 | public: 33 | enum StateNames{ 34 | v0, v1 35 | }; 36 | Noise(){}; 37 | ~Noise(){}; 38 | }; 39 | class ModelExample: public LWF::ModelBase{ 40 | public: 41 | ModelExample(){}; 42 | ~ModelExample(){}; 43 | void eval_(Output& output, const mtInputTuple& inputs, double dt) const{ 44 | const Input& input = std::get<0>(inputs); 45 | const Meas& meas = std::get<1>(inputs); 46 | const Noise& noise = std::get<2>(inputs); 47 | QPD dQ = dQ.exponentialMap(noise.get()); 48 | output.get() = (input.get().inverted()*input.get()).rotate(input.get())-input.get()+noise.get()-meas.get(); 49 | output.get() = meas.get().inverted()*dQ*input.get().inverted()*input.get(); 50 | } 51 | template::type* = nullptr> 52 | void jacInput_(Eigen::MatrixXd& J, const mtInputTuple& inputs, double dt) const{ 53 | const Input& input = std::get<0>(inputs); 54 | const Meas& meas = std::get<1>(inputs); 55 | const Noise& noise = std::get<2>(inputs); 56 | QPD dQ = dQ.exponentialMap(noise.get()); 57 | J.setZero(); 58 | J.block<3,3>(Output::getId(),Input::getId()) = -M3D::Identity(); 59 | J.block<3,3>(Output::getId(),Input::getId()) = MPD(input.get().inverted()*input.get()).matrix(); 60 | J.block<3,3>(Output::getId(),Input::getId()) = gSM((input.get().inverted()*input.get()).rotate(input.get()))*MPD(input.get().inverted()).matrix(); 61 | J.block<3,3>(Output::getId(),Input::getId()) = -gSM((input.get().inverted()*input.get()).rotate(input.get()))*MPD(input.get().inverted()).matrix(); 62 | J.block<3,3>(Output::getId(),Input::getId()) = MPD(meas.get().inverted()*dQ*input.get().inverted()).matrix(); 63 | J.block<3,3>(Output::getId(),Input::getId()) = -MPD(meas.get().inverted()*dQ*input.get().inverted()).matrix(); 64 | } 65 | template::type* = nullptr> 66 | void jacInput_(Eigen::MatrixXd& J, const mtInputTuple& inputs, double dt) const{ 67 | // Not done 68 | } 69 | template::type* = nullptr> 70 | void jacInput_(Eigen::MatrixXd& J, const mtInputTuple& inputs, double dt) const{ 71 | const Input& input = std::get<0>(inputs); 72 | const Meas& meas = std::get<1>(inputs); 73 | const Noise& noise = std::get<2>(inputs); 74 | QPD dQ = dQ.exponentialMap(noise.get()); 75 | J.setZero(); 76 | J.block<3,3>(Output::getId(),0) = M3D::Identity(); 77 | J.block<3,3>(Output::getId(),3) = MPD(meas.get().inverted()).matrix()*Lmat(noise.get()); 78 | } 79 | }; 80 | 81 | // The fixture for testing class PredictionModel 82 | class ModelBaseTest : public ::testing::Test { 83 | protected: 84 | ModelBaseTest() { 85 | unsigned int s = 0; 86 | testNoise_.setRandom(s); 87 | testInput_.setRandom(s); 88 | testMeas_.setRandom(s); 89 | } 90 | virtual ~ModelBaseTest(){} 91 | ModelExample model_; 92 | Input testInput_; 93 | Meas testMeas_; 94 | Noise testNoise_; 95 | }; 96 | 97 | // Test finite difference Jacobians 98 | TEST_F(ModelBaseTest, FDjacobians) { 99 | Eigen::MatrixXd F((int)(Output::D_),(int)(Input::D_)); 100 | Eigen::MatrixXd F_FD((int)(Output::D_),(int)(Input::D_)); 101 | model_.template jacInput<0>(F,std::forward_as_tuple(testInput_,testMeas_,testNoise_),0.1); 102 | model_.template jacInputFD<0>(F_FD,std::forward_as_tuple(testInput_,testMeas_,testNoise_),0.1,0.0000001); 103 | ASSERT_NEAR((F-F_FD).norm(),0.0,1e-5); 104 | Eigen::MatrixXd H((int)(Output::D_),(int)(Noise::D_)); 105 | Eigen::MatrixXd H_FD((int)(Output::D_),(int)(Noise::D_)); 106 | model_.template jacInput<2>(H,std::forward_as_tuple(testInput_,testMeas_,testNoise_),0.1); 107 | model_.template jacInputFD<2>(H_FD,std::forward_as_tuple(testInput_,testMeas_,testNoise_),0.1,0.0000001); 108 | ASSERT_NEAR((H-H_FD).norm(),0.0,1e-5); 109 | } 110 | 111 | int main(int argc, char **argv) { 112 | ::testing::InitGoogleTest(&argc, argv); 113 | return RUN_ALL_TESTS(); 114 | } 115 | -------------------------------------------------------------------------------- /src/testPrediction.cpp: -------------------------------------------------------------------------------- 1 | #include "lightweight_filtering/TestClasses.hpp" 2 | #include "lightweight_filtering/common.hpp" 3 | #include "gtest/gtest.h" 4 | #include 5 | 6 | using namespace LWFTest; 7 | 8 | typedef ::testing::Types< 9 | NonlinearTest, 10 | LinearTest 11 | > TestClasses; 12 | 13 | // The fixture for testing class PredictionModel 14 | template 15 | class PredictionModelTest : public ::testing::Test, public TestClass { 16 | protected: 17 | PredictionModelTest() { 18 | this->init(this->testState_,this->testUpdateMeas_,this->testPredictionMeas_); 19 | this->measMap_[0.1] = this->testPredictionMeas_; 20 | this->measMap_[0.2] = this->testPredictionMeas_; 21 | this->measMap_[0.4] = this->testPredictionMeas_; 22 | } 23 | virtual ~PredictionModelTest() { 24 | } 25 | using typename TestClass::mtState; 26 | using typename TestClass::mtFilterState; 27 | using typename TestClass::mtUpdateMeas; 28 | using typename TestClass::mtUpdateNoise; 29 | using typename TestClass::mtInnovation; 30 | using typename TestClass::mtPredictionNoise; 31 | using typename TestClass::mtPredictionMeas; 32 | using typename TestClass::mtUpdateExample; 33 | using typename TestClass::mtPredictionExample; 34 | using typename TestClass::mtPredictAndUpdateExample; 35 | mtPredictionExample testPrediction_; 36 | mtState testState_; 37 | mtPredictionMeas testPredictionMeas_; 38 | mtUpdateMeas testUpdateMeas_; 39 | const double dt_ = 0.1; 40 | std::map measMap_; 41 | }; 42 | 43 | TYPED_TEST_CASE(PredictionModelTest, TestClasses); 44 | 45 | // Test constructors 46 | TYPED_TEST(PredictionModelTest, constructors) { 47 | typename TestFixture::mtPredictionExample testPrediction; 48 | ASSERT_EQ((testPrediction.prenoiP_-Eigen::Matrix::Identity()*0.0001).norm(),0.0); 49 | } 50 | 51 | // Test finite difference Jacobians 52 | TYPED_TEST(PredictionModelTest, FDjacobians) { 53 | Eigen::MatrixXd F((int)(TestFixture::mtPredictionExample::mtState::D_),(int)(TestFixture::mtPredictionExample::mtState::D_)); 54 | Eigen::MatrixXd F_FD((int)(TestFixture::mtPredictionExample::mtState::D_),(int)(TestFixture::mtPredictionExample::mtState::D_)); 55 | typename TestFixture::mtPredictionNoise n; 56 | this->testPrediction_.meas_ = this->testPredictionMeas_; 57 | this->testPrediction_.template jacInputFD<0>(F_FD,std::forward_as_tuple(this->testState_,n),this->dt_,0.0000001); 58 | this->testPrediction_.template jacInput<0>(F,std::forward_as_tuple(this->testState_,n),this->dt_); 59 | Eigen::MatrixXd Fn((int)(TestFixture::mtPredictionExample::mtState::D_),(int)(TestFixture::mtPredictionExample::mtNoise::D_)); 60 | Eigen::MatrixXd Fn_FD((int)(TestFixture::mtPredictionExample::mtState::D_),(int)(TestFixture::mtPredictionExample::mtNoise::D_)); 61 | this->testPrediction_.template jacInputFD<1>(Fn_FD,std::forward_as_tuple(this->testState_,n),this->dt_,0.0000001); 62 | this->testPrediction_.template jacInput<1>(Fn,std::forward_as_tuple(this->testState_,n),this->dt_); 63 | switch(TestFixture::id_){ 64 | case 0: 65 | ASSERT_NEAR((F-F_FD).norm(),0.0,1e-5); 66 | ASSERT_NEAR((Fn-Fn_FD).norm(),0.0,1e-5); 67 | break; 68 | case 1: 69 | ASSERT_NEAR((F-F_FD).norm(),0.0,1e-8); 70 | ASSERT_NEAR((Fn-Fn_FD).norm(),0.0,1e-8); 71 | break; 72 | default: 73 | ASSERT_NEAR((F-F_FD).norm(),0.0,1e-5); 74 | ASSERT_NEAR((Fn-Fn_FD).norm(),0.0,1e-5); 75 | }; 76 | } 77 | 78 | // Test performPredictionEKF 79 | TYPED_TEST(PredictionModelTest, performPredictionEKF) { 80 | typename TestFixture::mtPredictionExample::mtFilterState filterState; 81 | filterState.cov_.setIdentity(); 82 | Eigen::MatrixXd F((int)(TestFixture::mtPredictionExample::mtState::D_),(int)(TestFixture::mtPredictionExample::mtState::D_)); 83 | this->testPrediction_.meas_ = this->testPredictionMeas_; 84 | this->testPrediction_.jacPreviousState(F,this->testState_,this->dt_); 85 | Eigen::MatrixXd Fn((int)(TestFixture::mtPredictionExample::mtState::D_),(int)(TestFixture::mtPredictionExample::mtNoise::D_)); 86 | this->testPrediction_.jacNoise(Fn,this->testState_,this->dt_); 87 | Eigen::MatrixXd predictedCov = F*filterState.cov_*F.transpose() + Fn*this->testPrediction_.prenoiP_*Fn.transpose(); 88 | typename TestFixture::mtPredictionExample::mtState state; 89 | filterState.state_ = this->testState_; 90 | this->testPrediction_.performPredictionEKF(filterState,this->testPredictionMeas_,this->dt_); 91 | typename TestFixture::mtPredictionExample::mtState::mtDifVec dif; 92 | typename TestFixture::mtPredictionExample::mtState evalState; 93 | this->testPrediction_.evalPredictionShort(evalState,this->testState_,this->dt_); 94 | filterState.state_.boxMinus(evalState,dif); 95 | switch(TestFixture::id_){ 96 | case 0: 97 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 98 | ASSERT_NEAR((filterState.cov_-predictedCov).norm(),0.0,1e-6); 99 | break; 100 | case 1: 101 | ASSERT_NEAR(dif.norm(),0.0,1e-10); 102 | ASSERT_NEAR((filterState.cov_-predictedCov).norm(),0.0,1e-10); 103 | break; 104 | default: 105 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 106 | ASSERT_NEAR((filterState.cov_-predictedCov).norm(),0.0,1e-6); 107 | }; 108 | } 109 | 110 | // Test comparePredict 111 | TYPED_TEST(PredictionModelTest, comparePredict) { 112 | typename TestFixture::mtPredictionExample::mtFilterState filterState1; 113 | typename TestFixture::mtPredictionExample::mtFilterState filterState2; 114 | filterState1.cov_.setIdentity(); 115 | filterState1.cov_ = filterState1.cov_*0.000001; 116 | filterState2.cov_ = filterState1.cov_; 117 | filterState1.state_ = this->testState_; 118 | filterState2.state_ = this->testState_; 119 | this->testPrediction_.performPredictionEKF(filterState1,this->testPredictionMeas_,this->dt_); 120 | this->testPrediction_.performPredictionUKF(filterState2,this->testPredictionMeas_,this->dt_); 121 | typename TestFixture::mtPredictionExample::mtState::mtDifVec dif; 122 | filterState1.state_.boxMinus(filterState2.state_,dif); 123 | switch(TestFixture::id_){ 124 | case 0: 125 | ASSERT_NEAR(dif.norm(),0.0,1e-5); 126 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-5); 127 | break; 128 | case 1: 129 | ASSERT_NEAR(dif.norm(),0.0,1e-9); 130 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-10); 131 | break; 132 | default: 133 | ASSERT_NEAR(dif.norm(),0.0,1e-5); 134 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-6); 135 | }; 136 | } 137 | 138 | // Test predictMergedEKF 139 | TYPED_TEST(PredictionModelTest, predictMergedEKF) { 140 | typename TestFixture::mtPredictionExample::mtFilterState filterState1; 141 | typename TestFixture::mtPredictionExample::mtFilterState filterState2; 142 | filterState1.cov_.setIdentity(); 143 | double t = 0; 144 | double dt = this->measMap_.rbegin()->first-t; 145 | 146 | typename TestFixture::mtPredictionExample::mtMeas meanMeas; 147 | typename TestFixture::mtPredictionExample::mtMeas::mtDifVec vec; 148 | typename TestFixture::mtPredictionExample::mtMeas::mtDifVec difVec; 149 | vec.setZero(); 150 | for(typename std::map::iterator it = next(this->measMap_.begin());it != this->measMap_.end();it++){ 151 | this->measMap_.begin()->second.boxMinus(it->second,difVec); 152 | vec = vec + difVec; 153 | } 154 | vec = vec/this->measMap_.size(); 155 | this->measMap_.begin()->second.boxPlus(vec,meanMeas); 156 | 157 | Eigen::MatrixXd F((int)(TestFixture::mtPredictionExample::mtState::D_),(int)(TestFixture::mtPredictionExample::mtState::D_)); 158 | this->testPrediction_.meas_ = meanMeas; 159 | this->testPrediction_.jacPreviousState(F,this->testState_,dt); 160 | Eigen::MatrixXd Fn((int)(TestFixture::mtPredictionExample::mtState::D_),(int)(TestFixture::mtPredictionExample::mtNoise::D_)); 161 | this->testPrediction_.jacNoise(Fn,this->testState_,dt); 162 | Eigen::MatrixXd predictedCov = F*filterState1.cov_*F.transpose() + Fn*this->testPrediction_.prenoiP_*Fn.transpose(); 163 | filterState1.state_ = this->testState_; 164 | this->testPrediction_.predictMergedEKF(filterState1,this->measMap_.rbegin()->first,this->measMap_); 165 | filterState2.state_ = this->testState_; 166 | for(typename std::map::iterator it = this->measMap_.begin();it != this->measMap_.end();it++){ 167 | this->testPrediction_.meas_ = it->second; 168 | this->testPrediction_.evalPredictionShort(filterState2.state_,filterState2.state_,it->first-t); 169 | t = it->first; 170 | } 171 | typename TestFixture::mtPredictionExample::mtState::mtDifVec dif; 172 | filterState1.state_.boxMinus(filterState2.state_,dif); 173 | switch(TestFixture::id_){ 174 | case 0: 175 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 176 | ASSERT_NEAR((filterState1.cov_-predictedCov).norm(),0.0,1e-6); 177 | break; 178 | case 1: 179 | ASSERT_NEAR(dif.norm(),0.0,1e-10); 180 | ASSERT_NEAR((filterState1.cov_-predictedCov).norm(),0.0,1e-10); 181 | break; 182 | default: 183 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 184 | ASSERT_NEAR((filterState1.cov_-predictedCov).norm(),0.0,1e-6); 185 | }; 186 | } 187 | 188 | // Test predictMergedUKF 189 | TYPED_TEST(PredictionModelTest, predictMergedUKF) { 190 | typename TestFixture::mtPredictionExample::mtFilterState filterState1; 191 | typename TestFixture::mtPredictionExample::mtFilterState filterState2; 192 | filterState1.cov_.setIdentity(); 193 | filterState2.cov_.setIdentity(); 194 | filterState1.state_ = this->testState_; 195 | filterState2.state_ = this->testState_; 196 | double t = 0; 197 | double dt = this->measMap_.rbegin()->first-t; 198 | 199 | typename TestFixture::mtPredictionExample::mtMeas meanMeas; 200 | typename TestFixture::mtPredictionExample::mtMeas::mtDifVec vec; 201 | typename TestFixture::mtPredictionExample::mtMeas::mtDifVec difVec; 202 | vec.setZero(); 203 | for(typename std::map::iterator it = next(this->measMap_.begin());it != this->measMap_.end();it++){ 204 | this->measMap_.begin()->second.boxMinus(it->second,difVec); 205 | vec = vec + difVec; 206 | } 207 | vec = vec/this->measMap_.size(); 208 | this->measMap_.begin()->second.boxPlus(vec,meanMeas); 209 | 210 | filterState1.stateSigmaPoints_.computeFromGaussian(filterState1.state_,filterState1.cov_); 211 | for(unsigned int i=0;itestPrediction_.meas_ = meanMeas; 213 | this->testPrediction_.evalPrediction(filterState1.stateSigmaPointsPre_(i),filterState1.stateSigmaPoints_(i),filterState1.stateSigmaPointsNoi_(i),dt); 214 | } 215 | filterState1.stateSigmaPointsPre_.getMean(filterState1.state_); 216 | filterState1.stateSigmaPointsPre_.getCovarianceMatrix(filterState1.state_,filterState1.cov_); 217 | this->testPrediction_.predictMergedUKF(filterState2,this->measMap_.rbegin()->first,this->measMap_); 218 | typename TestFixture::mtPredictionExample::mtState::mtDifVec dif; 219 | filterState1.state_.boxMinus(filterState2.state_,dif); 220 | switch(TestFixture::id_){ 221 | case 0: 222 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 223 | ASSERT_NEAR((filterState2.cov_-filterState1.cov_).norm(),0.0,1e-6); 224 | break; 225 | case 1: 226 | ASSERT_NEAR(dif.norm(),0.0,1e-10); 227 | ASSERT_NEAR((filterState2.cov_-filterState1.cov_).norm(),0.0,1e-10); 228 | break; 229 | default: 230 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 231 | ASSERT_NEAR((filterState2.cov_-filterState1.cov_).norm(),0.0,1e-6); 232 | }; 233 | } 234 | 235 | int main(int argc, char **argv) { 236 | ::testing::InitGoogleTest(&argc, argv); 237 | return RUN_ALL_TESTS(); 238 | } 239 | -------------------------------------------------------------------------------- /src/testSigmaPoints.cpp: -------------------------------------------------------------------------------- 1 | #include "lightweight_filtering/SigmaPoints.hpp" 2 | #include "lightweight_filtering/State.hpp" 3 | #include "lightweight_filtering/common.hpp" 4 | #include "gtest/gtest.h" 5 | #include 6 | 7 | // The fixture for testing class ScalarElement. 8 | class SigmaPointTest : public ::testing::Test { 9 | protected: 10 | SigmaPointTest(): P_((int)(mtState::D_),(int)(mtState::D_)), Qmat_((int)(mtState::D_),(int)(mtState::D_)){ 11 | sigmaPoints_.computeParameter(1e-3,2.0,0.0); 12 | sigmaPointsVector_.computeParameter(1e-3,2.0,0.0); 13 | mean_.template get<0>(0) = 4.5; 14 | for(int i=1;i(i) = mean_.template get<0>(i-1) + i*i*46.2; 16 | } 17 | mean_.template get<1>(0) << 2.1, -0.2, -1.9; 18 | for(int i=1;i(i) = mean_.template get<1>(i-1) + V3D(0.3,10.9,2.3); 20 | } 21 | mean_.template get<2>(0) = QPD(4.0/sqrt(30.0),3.0/sqrt(30.0),1.0/sqrt(30.0),2.0/sqrt(30.0)); 22 | for(int i=1;i(i) = mean_.template get<2>(i-1).boxPlus(mean_.template get<1>(i-1)); 24 | } 25 | // Easy way to obtain a pseudo random positive definite matrix 26 | P_ = mtState::D_*Eigen::MatrixXd::Identity((int)(mtState::D_),(int)(mtState::D_)); 27 | double randValue; 28 | for(int i=0;i qr(Qmat_); 44 | Qmat_ = qr.householderQ(); 45 | } 46 | ~SigmaPointTest() { 47 | } 48 | static const unsigned int S_ = 4; 49 | static const unsigned int V_ = 3; 50 | static const unsigned int Q_ = 2; 51 | static const unsigned int N_ = 2*(S_+3*(V_+Q_))+1; 52 | static const unsigned int O_ = 2; 53 | static const unsigned int L_ = N_+O_+2; 54 | typedef LWF::State,LWF::ArrayElement,V_>,LWF::ArrayElement> mtState; 55 | typedef LWF::VectorElement<3> mtElementVector; 56 | typedef mtState::mtDifVec mtDifVec; 57 | typedef LWF::SigmaPoints mtSigmaPoints; 58 | mtSigmaPoints sigmaPoints_; 59 | LWF::SigmaPoints sigmaPointsVector_; 60 | mtState mean_; 61 | Eigen::MatrixXd P_; 62 | Eigen::MatrixXd Qmat_; 63 | double alpha_ = 1e-3; 64 | double beta_ = 2.0; 65 | double kappa_ = 0.0; 66 | }; 67 | 68 | // Test constructors 69 | TEST_F(SigmaPointTest, constructors) { 70 | LWF::SigmaPoints sigmaPoints; 71 | ASSERT_TRUE(sigmaPoints.N_==N_); 72 | ASSERT_TRUE(sigmaPoints.L_==L_); 73 | ASSERT_TRUE(sigmaPoints.O_==O_); 74 | } 75 | 76 | // Test computeParameter 77 | TEST_F(SigmaPointTest, computeParameter) { 78 | sigmaPoints_.computeParameter(alpha_,beta_,kappa_); 79 | const unsigned int D = (L_-1)/2; 80 | double lambda = alpha_*alpha_*(D+kappa_)-D; 81 | double gamma = sqrt(lambda + D); 82 | double wm = 1/(2*(D+lambda)); 83 | double wc = wm; 84 | double wc0 = lambda/(D+lambda)+(1-alpha_*alpha_+beta_); 85 | ASSERT_EQ(sigmaPoints_.wm_,wm); 86 | ASSERT_EQ(sigmaPoints_.wc_,wc); 87 | ASSERT_EQ(sigmaPoints_.wc0_,wc0); 88 | ASSERT_EQ(sigmaPoints_.gamma_,gamma); 89 | } 90 | 91 | // Test computeFromGaussian, getMean, getCovariance, computeFromZeroMeanGaussian 92 | TEST_F(SigmaPointTest, computeFromGaussianPlusPlus) { 93 | // computeFromGaussian 94 | sigmaPoints_.computeFromGaussian(mean_,P_); 95 | 96 | // Check mean is same 97 | mtState mean; 98 | sigmaPoints_.getMean(mean); 99 | mtDifVec vec; 100 | mean.boxMinus(mean_,vec); 101 | ASSERT_NEAR(vec.norm(),0.0,1e-8); 102 | 103 | // Check covariance is same 104 | Eigen::MatrixXd P((int)(mtState::D_),(int)(mtState::D_)); 105 | sigmaPoints_.getCovarianceMatrix(mean,P); 106 | ASSERT_NEAR((P-P_).norm(),0.0,1e-8); 107 | 108 | // computeFromZeroMeanGaussian 109 | sigmaPoints_.computeFromZeroMeanGaussian(P_); 110 | 111 | // Check mean is same 112 | sigmaPoints_.getMean(mean); 113 | mean.boxMinus(mtState::Identity(),vec); 114 | ASSERT_NEAR(vec.norm(),0.0,1e-8); 115 | 116 | // Check covariance is same 117 | sigmaPoints_.getCovarianceMatrix(mean,P); 118 | ASSERT_NEAR((P-P_).norm(),0.0,1e-8); 119 | 120 | // Test with semidefinite matrix 121 | Eigen::MatrixXd Psemi = P_; 122 | Psemi.col(1) = Psemi.col(0); 123 | Psemi.row(1) = Psemi.row(0); 124 | sigmaPoints_.computeFromGaussian(mean_,Psemi); 125 | sigmaPoints_.getMean(mean); 126 | mean.boxMinus(mean_,vec); 127 | ASSERT_NEAR(vec.norm(),0.0,1e-8); 128 | sigmaPoints_.getCovarianceMatrix(mean,P); 129 | ASSERT_NEAR((P-Psemi).norm(),0.0,1e-8); 130 | } 131 | 132 | // Test computeFromGaussian with W matrix 133 | TEST_F(SigmaPointTest, computeFromGaussianQ) { 134 | // computeFromGaussian 135 | sigmaPoints_.computeFromGaussian(mean_,P_,Qmat_); 136 | 137 | // Check mean is same 138 | mtState mean; 139 | sigmaPoints_.getMean(mean); 140 | mtDifVec vec; 141 | mean.boxMinus(mean_,vec); 142 | ASSERT_NEAR(vec.norm(),0.0,1e-8); 143 | 144 | // Check covariance is same 145 | Eigen::MatrixXd P((int)(mtState::D_),(int)(mtState::D_)); 146 | sigmaPoints_.getCovarianceMatrix(mean,P); 147 | ASSERT_NEAR((P-P_).norm(),0.0,1e-8); 148 | } 149 | 150 | // Test getMean, getCovariance2 151 | TEST_F(SigmaPointTest, getCovariance2) { 152 | // computeFromGaussian 153 | sigmaPoints_.computeFromGaussian(mean_,P_); 154 | 155 | // Apply simple linear transformation 156 | for(int i=0;i(0)*2.45+V3D::Ones()*sigmaPoints_(i).template get<0>(0)*0.51; 158 | } 159 | 160 | Eigen::MatrixXd M((int)(mtElementVector::D_),(int)(mtState::D_)); 161 | sigmaPointsVector_.getCovarianceMatrix(sigmaPoints_,M); 162 | Eigen::MatrixXd H((int)(mtElementVector::D_),(int)(mtState::D_)); // Jacobian of linear transformation 163 | H.setZero(); 164 | H.block(0,S_,3,3) = M3D::Identity()*2.45; 165 | H.block(0,0,3,1) = V3D::Ones()*0.51; 166 | Eigen::Matrix Mref = H*P_; 167 | for(int i=0;i 5 | 6 | // The fixture for testing class ScalarState 7 | class ScalarElementTest : public virtual ::testing::Test { 8 | protected: 9 | ScalarElementTest():covMat_(1,1) { 10 | unsigned int s = 1; 11 | testElement1_.setRandom(s); 12 | testElement2_.setRandom(s); 13 | } 14 | virtual ~ScalarElementTest() { 15 | } 16 | LWF::ScalarElement testElement1_; 17 | LWF::ScalarElement testElement2_; 18 | LWF::ScalarElement::mtDifVec difVec_; 19 | MXD covMat_; 20 | }; 21 | 22 | // Test constructors 23 | TEST_F(ScalarElementTest, constructor) { 24 | LWF::ScalarElement testElement1; 25 | } 26 | 27 | // Test setIdentity and Identity 28 | TEST_F(ScalarElementTest, setIdentity) { 29 | testElement1_.setIdentity(); 30 | ASSERT_EQ(testElement1_.s_,0.0); 31 | ASSERT_EQ(LWF::ScalarElement::Identity().s_,0.0); 32 | } 33 | 34 | // Test plus and minus 35 | TEST_F(ScalarElementTest, plusAndMinus) { 36 | testElement2_.boxMinus(testElement1_,difVec_); 37 | ASSERT_EQ(difVec_(0),testElement2_.s_-testElement1_.s_); 38 | LWF::ScalarElement testElement3; 39 | testElement1_.boxPlus(difVec_,testElement3); 40 | ASSERT_NEAR(testElement2_.s_,testElement3.s_,1e-6); 41 | } 42 | 43 | // Test minus Jacobian 44 | TEST_F(ScalarElementTest, minusJac) { 45 | testElement2_.boxMinusJac(testElement1_,covMat_); 46 | ASSERT_NEAR((covMat_-Eigen::Matrix::Identity()).norm(),0.0,1e-6); 47 | } 48 | 49 | // Test getValue 50 | TEST_F(ScalarElementTest, accessors) { 51 | ASSERT_TRUE(testElement1_.get() == testElement1_.s_); 52 | } 53 | 54 | // Test operator= 55 | TEST_F(ScalarElementTest, operatorEQ) { 56 | testElement2_ = testElement1_; 57 | ASSERT_NEAR(testElement2_.s_,testElement1_.s_,1e-6); 58 | } 59 | 60 | // The fixture for testing class VectorState 61 | class VectorElementTest : public virtual ::testing::Test { 62 | protected: 63 | VectorElementTest():covMat_((int)N_,(int)N_) { 64 | unsigned int s = 1; 65 | testElement1_.setRandom(s); 66 | testElement2_.setRandom(s); 67 | } 68 | virtual ~VectorElementTest() { 69 | } 70 | static const unsigned int N_ = 4; 71 | LWF::VectorElement testElement1_; 72 | LWF::VectorElement testElement2_; 73 | LWF::VectorElement::mtDifVec difVec_; 74 | MXD covMat_; 75 | }; 76 | 77 | // Test constructors 78 | TEST_F(VectorElementTest, constructor) { 79 | LWF::VectorElement testElement1; 80 | } 81 | 82 | // Test setIdentity and Identity 83 | TEST_F(VectorElementTest, setIdentity) { 84 | testElement1_.setIdentity(); 85 | ASSERT_EQ(testElement1_.v_.norm(),0.0); 86 | ASSERT_EQ(LWF::VectorElement::Identity().v_.norm(),0.0); 87 | } 88 | 89 | // Test plus and minus 90 | TEST_F(VectorElementTest, plusAndMinus) { 91 | testElement2_.boxMinus(testElement1_,difVec_); 92 | ASSERT_NEAR((difVec_-(testElement2_.v_-testElement1_.v_)).norm(),0.0,1e-6); 93 | LWF::VectorElement testElement3; 94 | testElement1_.boxPlus(difVec_,testElement3); 95 | ASSERT_NEAR((testElement2_.v_-testElement3.v_).norm(),0.0,1e-6); 96 | } 97 | 98 | // Test minus Jacobian 99 | TEST_F(VectorElementTest, minusJac) { 100 | testElement2_.boxMinusJac(testElement1_,covMat_); 101 | ASSERT_NEAR((covMat_-Eigen::Matrix::Identity()).norm(),0.0,1e-6); 102 | } 103 | 104 | // Test getValue 105 | TEST_F(VectorElementTest, accessors) { 106 | ASSERT_TRUE(testElement1_.get() == testElement1_.v_); 107 | } 108 | 109 | // Test operator= 110 | TEST_F(VectorElementTest, operatorEQ) { 111 | testElement2_ = testElement1_; 112 | ASSERT_NEAR((testElement2_.v_-testElement1_.v_).norm(),0.0,1e-6); 113 | } 114 | 115 | // The fixture for testing class QuaternionElementTest 116 | class QuaternionElementTest : public virtual ::testing::Test { 117 | protected: 118 | QuaternionElementTest():covMat_(3,3) { 119 | unsigned int s = 1; 120 | testElement1_.setRandom(s); 121 | testElement2_.setRandom(s); 122 | } 123 | virtual ~QuaternionElementTest() { 124 | } 125 | LWF::QuaternionElement testElement1_; 126 | LWF::QuaternionElement testElement2_; 127 | LWF::QuaternionElement testElement3_; 128 | LWF::QuaternionElement::mtDifVec difVec_; 129 | LWF::QuaternionElement::mtDifVec difVecIn_; 130 | LWF::QuaternionElement::mtDifVec difVecOut1_; 131 | LWF::QuaternionElement::mtDifVec difVecOut2_; 132 | MXD covMat_; 133 | }; 134 | 135 | // Test constructors 136 | TEST_F(QuaternionElementTest, constructor) { 137 | LWF::QuaternionElement testElement1; 138 | } 139 | 140 | // Test setIdentity and Identity 141 | TEST_F(QuaternionElementTest, setIdentity) { 142 | testElement1_.setIdentity(); 143 | ASSERT_TRUE(testElement1_.q_.isNear(QPD(),1e-6)); 144 | ASSERT_TRUE(LWF::QuaternionElement::Identity().q_.isNear(QPD(),1e-6)); 145 | } 146 | 147 | // Test plus and minus 148 | TEST_F(QuaternionElementTest, plusAndMinus) { 149 | testElement2_.boxMinus(testElement1_,difVec_); 150 | ASSERT_NEAR((difVec_-testElement2_.q_.boxMinus(testElement1_.q_)).norm(),0.0,1e-6); 151 | LWF::QuaternionElement testElement3; 152 | testElement1_.boxPlus(difVec_,testElement3); 153 | ASSERT_TRUE(testElement2_.q_.isNear(testElement3.q_,1e-6)); 154 | } 155 | 156 | // Test minus Jacobian 157 | TEST_F(QuaternionElementTest, minusJac) { 158 | testElement2_.boxMinus(testElement1_,difVecOut1_); 159 | double d = 0.00001; 160 | testElement2_.boxMinusJac(testElement1_,covMat_); 161 | for(unsigned int i=0;i<3;i++){ 162 | difVecIn_.setZero(); 163 | difVecIn_(i) = d; 164 | testElement2_.boxPlus(difVecIn_,testElement3_); 165 | testElement3_.boxMinus(testElement1_,difVecOut2_); 166 | difVecOut2_ -= difVecOut1_; 167 | difVecOut2_ = difVecOut2_/d; 168 | ASSERT_NEAR((covMat_.col(i)-difVecOut2_).norm(),0.0,1e-5); 169 | } 170 | } 171 | 172 | // Test getValue 173 | TEST_F(QuaternionElementTest, accessors) { 174 | ASSERT_TRUE(testElement1_.get().isNear(testElement1_.q_,1e-6)); 175 | } 176 | 177 | // Test operator= 178 | TEST_F(QuaternionElementTest, operatorEQ) { 179 | testElement2_ = testElement1_; 180 | ASSERT_TRUE(testElement2_.q_.isNear(testElement1_.q_,1e-6)); 181 | } 182 | 183 | // Test LMat and derivative of rotation 184 | TEST_F(QuaternionElementTest, LMat) { 185 | double d = 0.00001; 186 | LWF::QuaternionElement att; 187 | LWF::VectorElement<3> vec; 188 | vec.v_ = V3D(0.4,-0.2,1.7); 189 | M3D J; 190 | LWF::QuaternionElement attDisturbed; 191 | LWF::VectorElement<3> vecDisturbed; 192 | M3D I; 193 | I.setIdentity(); 194 | V3D dif; 195 | I = d*I; 196 | att.q_ = att.q_.exponentialMap(vec.v_); 197 | for(unsigned int i=0;i<3;i++){ 198 | vec.boxPlus(I.col(i),vecDisturbed); 199 | attDisturbed.q_ = attDisturbed.q_.exponentialMap(vecDisturbed.v_); 200 | attDisturbed.boxMinus(att,dif); 201 | J.col(i) = dif*1/d; 202 | } 203 | ASSERT_NEAR((J-Lmat(vec.v_)).norm(),0.0,1e-5); 204 | } 205 | 206 | // Test LMat and derivative of rotation 207 | TEST_F(QuaternionElementTest, DerivativeOfRotation) { 208 | double d = 1e-6; 209 | LWF::QuaternionElement att; 210 | unsigned int s = 1; 211 | att.setRandom(s); 212 | LWF::QuaternionElement attDisturbed; 213 | M3D J; 214 | V3D dif; 215 | V3D a(2.1,0.7,-1.7); 216 | V3D b = att.q_.rotate(a); 217 | V3D b_disturbed; 218 | for(unsigned int i=0;i<3;i++){ 219 | dif.setZero(); dif(i) = d; 220 | att.boxPlus(dif,attDisturbed); 221 | b_disturbed = attDisturbed.q_.rotate(a); 222 | J.col(i) = (b_disturbed-b)/d; 223 | } 224 | ASSERT_NEAR((J+gSM(b)).norm(),0.0,1e-5); 225 | } 226 | 227 | // The fixture for testing class NormalVectorElementTest 228 | class NormalVectorElementTest : public virtual ::testing::Test { 229 | protected: 230 | NormalVectorElementTest():covMat_(2,2) { 231 | unsigned int s = 1; 232 | testElement1_.setRandom(s); 233 | testElement2_.setRandom(s); 234 | } 235 | virtual ~NormalVectorElementTest() { 236 | } 237 | LWF::NormalVectorElement testElement1_; 238 | LWF::NormalVectorElement testElement2_; 239 | LWF::NormalVectorElement testElement3_; 240 | LWF::NormalVectorElement::mtDifVec difVec_; 241 | LWF::NormalVectorElement::mtDifVec difVecIn_; 242 | LWF::NormalVectorElement::mtDifVec difVecOut1_; 243 | LWF::NormalVectorElement::mtDifVec difVecOut2_; 244 | MXD covMat_; 245 | }; 246 | 247 | // Test constructors 248 | TEST_F(NormalVectorElementTest, constructor) { 249 | LWF::NormalVectorElement testElement1; 250 | } 251 | 252 | // Test setIdentity and Identity 253 | TEST_F(NormalVectorElementTest, setIdentity) { 254 | testElement1_.setIdentity(); 255 | ASSERT_TRUE(testElement1_.getVec() == V3D(0,0,1)); 256 | ASSERT_TRUE(LWF::NormalVectorElement::Identity().getVec() == V3D(0,0,1)); 257 | } 258 | 259 | // Test plus and minus 260 | TEST_F(NormalVectorElementTest, plusAndMinus) { 261 | testElement2_.boxMinus(testElement1_,difVec_); 262 | LWF::NormalVectorElement testElement3; 263 | testElement1_.boxPlus(difVec_,testElement3); 264 | ASSERT_NEAR((testElement2_.getVec()-testElement3.getVec()).norm(),0.0,1e-6); 265 | } 266 | 267 | // Test minus Jacobian 268 | TEST_F(NormalVectorElementTest, minusJac) { 269 | testElement2_.boxMinus(testElement1_,difVecOut1_); 270 | double d = 0.00001; 271 | testElement2_.boxMinusJac(testElement1_,covMat_); 272 | for(unsigned int i=0;i<2;i++){ 273 | difVecIn_.setZero(); 274 | difVecIn_(i) = d; 275 | testElement2_.boxPlus(difVecIn_,testElement3_); 276 | testElement3_.boxMinus(testElement1_,difVecOut2_); 277 | difVecOut2_ -= difVecOut1_; 278 | difVecOut2_ = difVecOut2_/d; 279 | ASSERT_NEAR((covMat_.col(i)-difVecOut2_).norm(),0.0,1e-4); 280 | } 281 | } 282 | 283 | // Test getValue 284 | TEST_F(NormalVectorElementTest, accessors) { 285 | ASSERT_TRUE(testElement1_.get().getVec() == testElement1_.getVec()); 286 | } 287 | 288 | // Test operator= 289 | TEST_F(NormalVectorElementTest, operatorEQ) { 290 | testElement2_ = testElement1_; 291 | ASSERT_TRUE(testElement2_.getVec() == testElement1_.getVec()); 292 | } 293 | 294 | // Test getPerpendiculars 295 | TEST_F(NormalVectorElementTest, getTwoNormals) { 296 | ASSERT_NEAR(testElement1_.getPerp1().dot(testElement1_.getVec()),0.0,1e-6); 297 | ASSERT_NEAR(testElement1_.getPerp2().dot(testElement1_.getVec()),0.0,1e-6); 298 | ASSERT_NEAR(testElement1_.getPerp2().dot(testElement1_.getPerp1()),0.0,1e-6); 299 | } 300 | 301 | // Test set from vector 302 | TEST_F(NormalVectorElementTest, setFromVector) { 303 | testElement2_.setFromVector(testElement1_.getVec()); 304 | ASSERT_NEAR((testElement2_.getVec()-testElement1_.getVec()).norm(),0.0,1e-6); 305 | } 306 | 307 | // Test rotated 308 | TEST_F(NormalVectorElementTest, rotated) { 309 | LWF::QuaternionElement q; 310 | unsigned int s = 1; 311 | q.setRandom(s); 312 | testElement2_.setFromVector(testElement1_.getVec()); 313 | ASSERT_NEAR((q.q_.rotate(testElement1_.getVec())-testElement1_.rotated(q.q_).getVec()).norm(),0.0,1e-6); 314 | } 315 | 316 | // Test inverted 317 | TEST_F(NormalVectorElementTest, inverted) { 318 | ASSERT_NEAR((testElement1_.getVec()+testElement1_.inverted().getVec()).norm(),0.0,1e-6); 319 | } 320 | 321 | // Test derivative of vector 322 | TEST_F(NormalVectorElementTest, derivative) { 323 | const double d = 1e-6; 324 | difVec_.setZero(); 325 | difVec_(0) = d; 326 | testElement1_.boxPlus(difVec_,testElement2_); 327 | ASSERT_NEAR(((testElement2_.getVec()-testElement1_.getVec())/d-testElement1_.getM().col(0)).norm(),0.0,1e-6); 328 | difVec_.setZero(); 329 | difVec_(1) = d; 330 | testElement1_.boxPlus(difVec_,testElement2_); 331 | ASSERT_NEAR(((testElement2_.getVec()-testElement1_.getVec())/d-testElement1_.getM().col(1)).norm(),0.0,1e-6); 332 | } 333 | 334 | // Test getRotationFromTwoNormals 335 | TEST_F(NormalVectorElementTest, getRotationFromTwoNormals) { 336 | V3D theta = LWF::NormalVectorElement::getRotationFromTwoNormals(testElement1_,testElement2_); 337 | QPD q = q.exponentialMap(theta); 338 | ASSERT_NEAR((testElement1_.rotated(q).getVec()-testElement2_.getVec()).norm(),0.0,1e-6); 339 | ASSERT_NEAR((LWF::NormalVectorElement::getRotationFromTwoNormals(testElement1_,testElement2_)+LWF::NormalVectorElement::getRotationFromTwoNormals(testElement2_,testElement1_)).norm(),0.0,1e-6); 340 | } 341 | 342 | // Test getRotationFromTwoNormalsJac 343 | TEST_F(NormalVectorElementTest, getRotationFromTwoNormalsJac) { 344 | V3D testVec1 = testElement1_.getVec(); 345 | V3D testVec2 = testElement2_.getVec(); 346 | V3D perp = testElement1_.getPerp1(); 347 | V3D theta = LWF::NormalVectorElement::getRotationFromTwoNormals(testVec1,testVec2,perp); 348 | covMat_ = LWF::NormalVectorElement::getRotationFromTwoNormalsJac(testVec1,testVec2); 349 | 350 | double d = 0.00001; 351 | for(unsigned int i=0;i<3;i++){ 352 | V3D testVec3 = testVec1; 353 | testVec3(i) += d; 354 | V3D theta2 = LWF::NormalVectorElement::getRotationFromTwoNormals(testVec3,testVec2,perp); 355 | V3D diff = (theta2-theta)/d; 356 | ASSERT_NEAR((covMat_.col(i)-diff).norm(),0.0,1e-4); 357 | } 358 | } 359 | 360 | // Test derivative of boxminus (includes test of derivative of getRotationFromTwoNormals) 361 | TEST_F(NormalVectorElementTest, derivativeBoxMinus) { 362 | const double d = 1e-6; 363 | Eigen::Matrix2d J; 364 | LWF::NormalVectorElement::mtDifVec perturbation; 365 | LWF::NormalVectorElement::mtDifVec out; 366 | LWF::NormalVectorElement::mtDifVec out_perturbed; 367 | LWF::NormalVectorElement testElement2_perturbed; 368 | testElement2_.boxMinus(testElement1_,out); 369 | perturbation.setZero(); 370 | perturbation(0) = d; 371 | testElement2_.boxPlus(perturbation,testElement2_perturbed); 372 | testElement2_perturbed.boxMinus(testElement1_,out_perturbed); 373 | J.col(0) = (out_perturbed-out)/d; 374 | perturbation.setZero(); 375 | perturbation(1) = d; 376 | testElement2_.boxPlus(perturbation,testElement2_perturbed); 377 | testElement2_perturbed.boxMinus(testElement1_,out_perturbed); 378 | J.col(1) = (out_perturbed-out)/d; 379 | ASSERT_NEAR((testElement1_.getN().transpose()*-LWF::NormalVectorElement::getRotationFromTwoNormalsJac(testElement2_,testElement1_)*testElement2_.getM()-J).norm(),0.0,1e-5); 380 | } 381 | 382 | // The fixture for testing class ArrayElementTest 383 | class ArrayElementTest : public virtual ::testing::Test { 384 | protected: 385 | ArrayElementTest():covMat_((int)N_*3,(int)N_*3) { 386 | unsigned int s = 1; 387 | testElement1_.setRandom(s); 388 | testElement2_.setRandom(s); 389 | } 390 | virtual ~ArrayElementTest() { 391 | } 392 | static const unsigned int N_ = 5; 393 | LWF::ArrayElement testElement1_; 394 | LWF::ArrayElement testElement2_; 395 | LWF::ArrayElement testElement3_; 396 | LWF::ArrayElement::mtDifVec difVec_; 397 | LWF::ArrayElement::mtDifVec difVecIn_; 398 | LWF::ArrayElement::mtDifVec difVecOut1_; 399 | LWF::ArrayElement::mtDifVec difVecOut2_; 400 | MXD covMat_; 401 | }; 402 | 403 | // Test constructors 404 | TEST_F(ArrayElementTest, constructor) { 405 | LWF::ArrayElement testElement1; 406 | } 407 | 408 | // Test setIdentity and Identity 409 | TEST_F(ArrayElementTest, setIdentity) { 410 | testElement1_.setIdentity(); 411 | for(unsigned int i=0;i::Identity().array_[i].q_.isNear(QPD(),1e-6))); 414 | } 415 | } 416 | 417 | // Test plus and minus 418 | TEST_F(ArrayElementTest, plusAndMinus) { 419 | testElement2_.boxMinus(testElement1_,difVec_); 420 | for(unsigned int i=0;i(i*3,0)-testElement2_.array_[i].q_.boxMinus(testElement1_.array_[i].q_)).norm(),0.0,1e-6); 422 | } 423 | LWF::ArrayElement testElement3; 424 | testElement1_.boxPlus(difVec_,testElement3); 425 | for(unsigned int i=0;i{ 462 | public: 463 | AuxillaryElement(){ 464 | x_ = 1.0; 465 | }; 466 | ~AuxillaryElement(){}; 467 | double x_; 468 | }; 469 | 470 | class StateTesting : public virtual ::testing::Test { 471 | protected: 472 | static const unsigned int _sca = 0; 473 | static const unsigned int _vec0 = _sca+1; 474 | static const unsigned int _vec1 = _vec0+1; 475 | static const unsigned int _vec2 = _vec1+1; 476 | static const unsigned int _vec3 = _vec2+1; 477 | static const unsigned int _qua0 = _vec3+1; 478 | static const unsigned int _qua1 = _qua0+1; 479 | static const unsigned int _aux = _qua1+1; 480 | StateTesting():covMat_((int)mtState::D_,(int)mtState::D_) { 481 | testScalar1_ = 4.5; 482 | testScalar2_ = -17.34; 483 | testVector1_[0] << 2.1, -0.2, -1.9; 484 | testVector2_[0] << -10.6, 0.2, -105.2; 485 | for(int i=1;i<4;i++){ 486 | testVector1_[i] = testVector1_[i-1] + V3D(0.3,10.9,2.3); 487 | testVector2_[i] = testVector2_[i-1] + V3D(-1.5,12,1785.23); 488 | } 489 | testQuat1_[0] = QPD(4.0/sqrt(30.0),3.0/sqrt(30.0),1.0/sqrt(30.0),2.0/sqrt(30.0)); 490 | testQuat2_[0] = QPD(0.0,0.36,0.48,0.8); 491 | for(int i=1;i<4;i++){ 492 | testQuat1_[i] = testQuat1_[i-1].boxPlus(testVector1_[i-1]); 493 | testQuat2_[i] = testQuat2_[i-1].boxPlus(testVector2_[i-1]); 494 | } 495 | testState1_.get<_sca>() = testScalar1_; 496 | testState1_.get<_vec0>() = testVector1_[0]; 497 | testState1_.get<_vec1>() = testVector1_[1]; 498 | testState1_.get<_vec2>() = testVector1_[2]; 499 | testState1_.get<_vec3>() = testVector1_[3]; 500 | testState1_.get<_qua0>(0) = testQuat1_[0]; 501 | testState1_.get<_qua0>(1) = testQuat1_[1]; 502 | testState1_.get<_qua1>(0) = testQuat1_[2]; 503 | testState1_.get<_qua1>(1) = testQuat1_[3]; 504 | testState1_.get<_aux>().x_ = 2.3; 505 | testState2_.get<_sca>() = testScalar2_; 506 | testState2_.get<_vec0>() = testVector2_[0]; 507 | testState2_.get<_vec1>() = testVector2_[1]; 508 | testState2_.get<_vec2>() = testVector2_[2]; 509 | testState2_.get<_vec3>() = testVector2_[3]; 510 | testState2_.get<_qua0>(0) = testQuat2_[0]; 511 | testState2_.get<_qua0>(1) = testQuat2_[1]; 512 | testState2_.get<_qua1>(0) = testQuat2_[2]; 513 | testState2_.get<_qua1>(1) = testQuat2_[3]; 514 | testState2_.get<_aux>().x_ = 3.2; 515 | } 516 | virtual ~StateTesting() { 517 | } 518 | typedef LWF::State< 519 | LWF::ScalarElement, 520 | LWF::TH_multiple_elements,4>, 521 | LWF::TH_multiple_elements,2>, 522 | AuxillaryElement> mtState; 523 | mtState testState1_; 524 | mtState testState2_; 525 | mtState testState3_; 526 | mtState::mtDifVec difVec_; 527 | mtState::mtDifVec difVecIn_; 528 | mtState::mtDifVec difVecOut1_; 529 | mtState::mtDifVec difVecOut2_; 530 | MXD covMat_; 531 | double testScalar1_; 532 | double testScalar2_; 533 | V3D testVector1_[4]; 534 | V3D testVector2_[4]; 535 | QPD testQuat1_[4]; 536 | QPD testQuat2_[4]; 537 | }; 538 | 539 | // Test constructors 540 | TEST_F(StateTesting, constructors) { 541 | mtState testState1; 542 | ASSERT_EQ(testState1.get<_aux>().x_,1.0); 543 | mtState testState2(testState2_); 544 | testState2.boxMinus(testState2_,difVec_); 545 | ASSERT_NEAR(difVec_.norm(),0.0,1e-6); 546 | ASSERT_EQ(testState2.get<_aux>().x_,3.2); 547 | } 548 | 549 | // Test setIdentity and Identity 550 | TEST_F(StateTesting, setIdentity) { 551 | testState1_.setIdentity(); 552 | ASSERT_EQ(testState1_.get<_sca>(),0); 553 | ASSERT_EQ(testState1_.get<_vec0>().norm(),0); 554 | ASSERT_EQ(testState1_.get<_vec1>().norm(),0); 555 | ASSERT_EQ(testState1_.get<_vec2>().norm(),0); 556 | ASSERT_EQ(testState1_.get<_vec3>().norm(),0); 557 | ASSERT_EQ(testState1_.get<_qua0>(0).boxMinus(QPD()).norm(),0.0); 558 | ASSERT_EQ(testState1_.get<_qua0>(1).boxMinus(QPD()).norm(),0.0); 559 | ASSERT_EQ(testState1_.get<_qua1>(0).boxMinus(QPD()).norm(),0.0); 560 | ASSERT_EQ(testState1_.get<_qua1>(1).boxMinus(QPD()).norm(),0.0); 561 | ASSERT_EQ(testState1_.get<_aux>().x_,2.3); 562 | ASSERT_EQ(mtState::Identity().get<_sca>(),0); 563 | ASSERT_EQ(mtState::Identity().get<_vec0>().norm(),0); 564 | ASSERT_EQ(mtState::Identity().get<_vec1>().norm(),0); 565 | ASSERT_EQ(mtState::Identity().get<_vec2>().norm(),0); 566 | ASSERT_EQ(mtState::Identity().get<_vec3>().norm(),0); 567 | ASSERT_EQ(mtState::Identity().get<_qua0>(0).boxMinus(QPD()).norm(),0.0); 568 | ASSERT_EQ(mtState::Identity().get<_qua0>(1).boxMinus(QPD()).norm(),0.0); 569 | ASSERT_EQ(mtState::Identity().get<_qua1>(0).boxMinus(QPD()).norm(),0.0); 570 | ASSERT_EQ(mtState::Identity().get<_qua1>(1).boxMinus(QPD()).norm(),0.0); 571 | ASSERT_EQ(mtState::Identity().get<_aux>().x_,1.0); 572 | } 573 | 574 | // Test plus and minus 575 | TEST_F(StateTesting, plusAndMinus) { 576 | testState2_.boxMinus(testState1_,difVec_); 577 | unsigned int index=0; 578 | ASSERT_EQ(difVec_(index),testScalar2_-testScalar1_); 579 | index ++; 580 | for(int i=0;i<4;i++){ 581 | ASSERT_EQ((difVec_.block<3,1>(index,0)-(testVector2_[i]-testVector1_[i])).norm(),0); 582 | index = index + 3; 583 | } 584 | for(int i=0;i<4;i++){ 585 | ASSERT_EQ((difVec_.block<3,1>(index,0)-testQuat2_[i].boxMinus(testQuat1_[i])).norm(),0); 586 | index = index + 3; 587 | } 588 | testState1_.boxPlus(difVec_,testState2_); 589 | ASSERT_NEAR(testState2_.get<_sca>(),testScalar2_,1e-10); 590 | ASSERT_NEAR((testState2_.get<_vec0>()-testVector2_[0]).norm(),0,1e-10); 591 | ASSERT_NEAR((testState2_.get<_vec1>()-testVector2_[1]).norm(),0,1e-10); 592 | ASSERT_NEAR((testState2_.get<_vec2>()-testVector2_[2]).norm(),0,1e-10); 593 | ASSERT_NEAR((testState2_.get<_vec3>()-testVector2_[3]).norm(),0,1e-10); 594 | ASSERT_NEAR(testState2_.get<_qua0>(0).boxMinus(testQuat2_[0]).norm(),0,1e-10); 595 | ASSERT_NEAR(testState2_.get<_qua0>(1).boxMinus(testQuat2_[1]).norm(),0,1e-10); 596 | ASSERT_NEAR(testState2_.get<_qua1>(0).boxMinus(testQuat2_[2]).norm(),0,1e-10); 597 | ASSERT_NEAR(testState2_.get<_qua1>(1).boxMinus(testQuat2_[3]).norm(),0,1e-10); 598 | ASSERT_EQ(testState2_.get<_aux>().x_,testState1_.get<_aux>().x_); 599 | } 600 | 601 | // Test minus Jacobian 602 | TEST_F(StateTesting, minusJac) { 603 | testState2_.boxMinus(testState1_,difVecOut1_); 604 | double d = 0.00001; 605 | testState2_.boxMinusJac(testState1_,covMat_); 606 | for(unsigned int i=0;i(),testScalar1_,1e-10); 620 | ASSERT_NEAR((testState1_.get<_vec0>()-testVector1_[0]).norm(),0,1e-10); 621 | ASSERT_NEAR((testState1_.get<_vec1>()-testVector1_[1]).norm(),0,1e-10); 622 | ASSERT_NEAR((testState1_.get<_vec2>()-testVector1_[2]).norm(),0,1e-10); 623 | ASSERT_NEAR((testState1_.get<_vec3>()-testVector1_[3]).norm(),0,1e-10); 624 | ASSERT_NEAR(testState1_.get<_qua0>(0).boxMinus(testQuat1_[0]).norm(),0,1e-10); 625 | ASSERT_NEAR(testState1_.get<_qua0>(1).boxMinus(testQuat1_[1]).norm(),0,1e-10); 626 | ASSERT_NEAR(testState1_.get<_qua1>(0).boxMinus(testQuat1_[2]).norm(),0,1e-10); 627 | ASSERT_NEAR(testState1_.get<_qua1>(1).boxMinus(testQuat1_[3]).norm(),0,1e-10); 628 | ASSERT_EQ(testState1_.get<_aux>().x_,2.3); 629 | 630 | ASSERT_TRUE(testState1_.getId<_sca>() == 0); 631 | ASSERT_TRUE(testState1_.getId<_vec0>() == 1); 632 | ASSERT_TRUE(testState1_.getId<_vec1>() == 4); 633 | ASSERT_TRUE(testState1_.getId<_vec2>() == 7); 634 | ASSERT_TRUE(testState1_.getId<_vec3>() == 10); 635 | ASSERT_TRUE(testState1_.getId<_qua0>(0) == 13); 636 | ASSERT_TRUE(testState1_.getId<_qua0>(1) == 16); 637 | ASSERT_TRUE(testState1_.getId<_qua1>(0) == 19); 638 | ASSERT_TRUE(testState1_.getId<_qua1>(1) == 22); 639 | ASSERT_TRUE(testState1_.getId<_aux>() == 25); 640 | 641 | ASSERT_TRUE(testState1_.getElementId(0) == _sca); 642 | ASSERT_TRUE(testState1_.getElementId(1) == _vec0); 643 | ASSERT_TRUE(testState1_.getElementId(4) == _vec1); 644 | ASSERT_TRUE(testState1_.getElementId(7) == _vec2); 645 | ASSERT_TRUE(testState1_.getElementId(10) == _vec3); 646 | ASSERT_TRUE(testState1_.getElementId(13) == _qua0); 647 | ASSERT_TRUE(testState1_.getElementId(16) == _qua0); 648 | ASSERT_TRUE(testState1_.getElementId(19) == _qua1); 649 | ASSERT_TRUE(testState1_.getElementId(22) == _qua1); 650 | std::cout << "CHECK (ERROR: Exceeded state size)" << std::endl; 651 | ASSERT_TRUE(testState1_.getElementId(25) == _aux+1); // Exceeds state size 652 | } 653 | 654 | // Test operator= 655 | TEST_F(StateTesting, operatorEQ) { 656 | testState2_ = testState1_; 657 | ASSERT_NEAR(testState2_.get<_sca>(),testScalar1_,1e-10); 658 | ASSERT_NEAR((testState2_.get<_vec0>()-testVector1_[0]).norm(),0,1e-10); 659 | ASSERT_NEAR((testState2_.get<_vec1>()-testVector1_[1]).norm(),0,1e-10); 660 | ASSERT_NEAR((testState2_.get<_vec2>()-testVector1_[2]).norm(),0,1e-10); 661 | ASSERT_NEAR((testState2_.get<_vec3>()-testVector1_[3]).norm(),0,1e-10); 662 | ASSERT_NEAR(testState2_.get<_qua0>(0).boxMinus(testQuat1_[0]).norm(),0,1e-10); 663 | ASSERT_NEAR(testState2_.get<_qua0>(1).boxMinus(testQuat1_[1]).norm(),0,1e-10); 664 | ASSERT_NEAR(testState2_.get<_qua1>(0).boxMinus(testQuat1_[2]).norm(),0,1e-10); 665 | ASSERT_NEAR(testState2_.get<_qua1>(1).boxMinus(testQuat1_[3]).norm(),0,1e-10); 666 | ASSERT_EQ(testState2_.get<_aux>().x_,2.3); 667 | } 668 | 669 | // Test createDefaultNames 670 | TEST_F(StateTesting, naming) { 671 | testState1_.createDefaultNames("test"); 672 | ASSERT_TRUE(testState1_.getName<_sca>() == "test_0"); 673 | ASSERT_TRUE(testState1_.getName<_vec0>() == "test_1"); 674 | ASSERT_TRUE(testState1_.getName<_vec1>() == "test_2"); 675 | ASSERT_TRUE(testState1_.getName<_vec2>() == "test_3"); 676 | ASSERT_TRUE(testState1_.getName<_vec3>() == "test_4"); 677 | ASSERT_TRUE(testState1_.getName<_qua0>() == "test_5"); 678 | ASSERT_TRUE(testState1_.getName<_qua1>() == "test_6"); 679 | ASSERT_TRUE(testState1_.getName<_aux>() == "test_7"); 680 | } 681 | 682 | // Test ZeroArray 683 | TEST_F(StateTesting, ZeroArray) { 684 | LWF::State,LWF::QuaternionElement> testState1; 685 | ASSERT_EQ(std::tuple_size::value,1); 686 | } 687 | 688 | // Test Constness 689 | TEST_F(StateTesting, Constness) { 690 | const mtState testState1(testState1_); 691 | ASSERT_NEAR(testState1.get<_qua0>(0).boxMinus(testQuat1_[0]).norm(),0,1e-10); 692 | } 693 | 694 | int main(int argc, char **argv) { 695 | ::testing::InitGoogleTest(&argc, argv); 696 | return RUN_ALL_TESTS(); 697 | } 698 | -------------------------------------------------------------------------------- /src/testUpdate.cpp: -------------------------------------------------------------------------------- 1 | #include "lightweight_filtering/TestClasses.hpp" 2 | #include "lightweight_filtering/common.hpp" 3 | #include "gtest/gtest.h" 4 | #include 5 | 6 | using namespace LWFTest; 7 | 8 | typedef ::testing::Types< 9 | NonlinearTest, 10 | LinearTest 11 | > TestClasses; 12 | 13 | // The fixture for testing class UpdateModel 14 | template 15 | class UpdateModelTest : public ::testing::Test, public TestClass { 16 | public: 17 | UpdateModelTest() { 18 | this->init(this->testState_,this->testUpdateMeas_,this->testPredictionMeas_); 19 | } 20 | virtual ~UpdateModelTest() { 21 | } 22 | using typename TestClass::mtState; 23 | using typename TestClass::mtFilterState; 24 | using typename TestClass::mtUpdateMeas; 25 | using typename TestClass::mtUpdateNoise; 26 | using typename TestClass::mtInnovation; 27 | using typename TestClass::mtPredictionNoise; 28 | using typename TestClass::mtPredictionMeas; 29 | using typename TestClass::mtUpdateExample; 30 | using typename TestClass::mtPredictionExample; 31 | using typename TestClass::mtPredictAndUpdateExample; 32 | using typename TestClass::mtOutlierDetectionExample; 33 | mtUpdateExample testUpdate_; 34 | mtPredictAndUpdateExample testPredictAndUpdate_; 35 | mtPredictionExample testPrediction_; 36 | mtState testState_; 37 | mtUpdateMeas testUpdateMeas_; 38 | mtPredictionMeas testPredictionMeas_; 39 | const double dt_ = 0.1; 40 | }; 41 | 42 | TYPED_TEST_CASE(UpdateModelTest, TestClasses); 43 | 44 | // Test constructors 45 | TYPED_TEST(UpdateModelTest, constructors) { 46 | typename TestFixture::mtUpdateExample testUpdate; 47 | bool coupledToPrediction = testUpdate.coupledToPrediction_; 48 | ASSERT_EQ(coupledToPrediction,false); 49 | ASSERT_EQ((testUpdate.updnoiP_-Eigen::Matrix::Identity()*0.0001).norm(),0.0); 50 | typename TestFixture::mtUpdateExample::mtNoise::mtDifVec dif; 51 | typename TestFixture::mtUpdateExample::mtNoise noise; 52 | noise.setIdentity(); 53 | typename TestFixture::mtUpdateExample::mtNoise mean; 54 | testUpdate.stateSigmaPointsNoi_.getMean(mean); 55 | mean.boxMinus(noise,dif); 56 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 57 | Eigen::MatrixXd C((int)(TestFixture::mtUpdateExample::mtNoise::D_),(int)(TestFixture::mtUpdateExample::mtNoise::D_)); 58 | testUpdate.stateSigmaPointsNoi_.getCovarianceMatrix(C); 59 | ASSERT_NEAR((testUpdate.updnoiP_-C).norm(),0.0,1e-8); 60 | typename TestFixture::mtPredictAndUpdateExample testPredictAndUpdate; 61 | coupledToPrediction = testPredictAndUpdate.coupledToPrediction_; 62 | ASSERT_EQ(coupledToPrediction,true); 63 | ASSERT_EQ((testPredictAndUpdate.updnoiP_-Eigen::Matrix::Identity()*0.0001).norm(),0.0); 64 | } 65 | 66 | // Test finite difference Jacobians 67 | TYPED_TEST(UpdateModelTest, FDjacobians) { 68 | typename TestFixture::mtUpdateExample::mtNoise n; 69 | n.setIdentity(); 70 | this->testUpdate_.meas_ = this->testUpdateMeas_; 71 | Eigen::MatrixXd F((int)(TestFixture::mtUpdateExample::mtInnovation::D_),(int)(TestFixture::mtUpdateExample::mtState::D_)); 72 | Eigen::MatrixXd F_FD((int)(TestFixture::mtUpdateExample::mtInnovation::D_),(int)(TestFixture::mtUpdateExample::mtState::D_)); 73 | this->testUpdate_.template jacInputFD<0>(F_FD,std::forward_as_tuple(this->testState_,n),this->dt_,0.0000001); 74 | this->testUpdate_.template jacInput<0>(F,std::forward_as_tuple(this->testState_,n)); 75 | Eigen::MatrixXd H((int)(TestFixture::mtUpdateExample::mtInnovation::D_),(int)(TestFixture::mtUpdateExample::mtNoise::D_)); 76 | Eigen::MatrixXd H_FD((int)(TestFixture::mtUpdateExample::mtInnovation::D_),(int)(TestFixture::mtUpdateExample::mtNoise::D_)); 77 | this->testUpdate_.template jacInputFD<1>(H_FD,std::forward_as_tuple(this->testState_,n),this->dt_,0.0000001); 78 | this->testUpdate_.template jacInput<1>(H,std::forward_as_tuple(this->testState_,n)); 79 | switch(TestFixture::id_){ 80 | case 0: 81 | ASSERT_NEAR((F-F_FD).norm(),0.0,1e-5); 82 | ASSERT_NEAR((H-H_FD).norm(),0.0,1e-5); 83 | break; 84 | case 1: 85 | ASSERT_NEAR((F-F_FD).norm(),0.0,1e-8); 86 | ASSERT_NEAR((H-H_FD).norm(),0.0,1e-8); 87 | break; 88 | default: 89 | ASSERT_NEAR((F-F_FD).norm(),0.0,1e-5); 90 | ASSERT_NEAR((H-H_FD).norm(),0.0,1e-5); 91 | } 92 | } 93 | 94 | // Test performUpdateEKF 95 | TYPED_TEST(UpdateModelTest, performUpdateEKF) { 96 | this->testUpdate_.meas_ = this->testUpdateMeas_; 97 | typename TestFixture::mtUpdateExample::mtFilterState filterState; 98 | filterState.cov_.setIdentity(); 99 | Eigen::MatrixXd H((int)(TestFixture::mtUpdateExample::mtInnovation::D_),(int)(TestFixture::mtUpdateExample::mtState::D_)); 100 | this->testUpdate_.jacState(H,this->testState_); 101 | Eigen::MatrixXd Hn((int)(TestFixture::mtUpdateExample::mtInnovation::D_),(int)(TestFixture::mtUpdateExample::mtNoise::D_)); 102 | this->testUpdate_.jacNoise(Hn,this->testState_); 103 | 104 | typename TestFixture::mtUpdateExample::mtInnovation y; 105 | this->testUpdate_.evalInnovationShort(y,this->testState_); 106 | typename TestFixture::mtUpdateExample::mtInnovation yIdentity; 107 | yIdentity.setIdentity(); 108 | typename TestFixture::mtUpdateExample::mtInnovation::mtDifVec innVector; 109 | 110 | typename TestFixture::mtUpdateExample::mtState stateUpdated; 111 | filterState.state_ = this->testState_; 112 | 113 | // Update 114 | MXD Py = H*filterState.cov_*H.transpose() + Hn*this->testUpdate_.updnoiP_*Hn.transpose(); 115 | y.boxMinus(yIdentity,innVector); 116 | MXD Pyinv = Py.inverse(); 117 | 118 | // Kalman Update 119 | Eigen::Matrix K = filterState.cov_*H.transpose()*Pyinv; 120 | MXD updateCov = filterState.cov_ - K*Py*K.transpose(); 121 | typename TestFixture::mtUpdateExample::mtState::mtDifVec updateVec; 122 | updateVec = -K*innVector; 123 | filterState.state_.boxPlus(updateVec,stateUpdated); 124 | 125 | this->testUpdate_.performUpdateEKF(filterState,this->testUpdateMeas_); 126 | typename TestFixture::mtUpdateExample::mtState::mtDifVec dif; 127 | filterState.state_.boxMinus(stateUpdated,dif); 128 | switch(TestFixture::id_){ 129 | case 0: 130 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 131 | ASSERT_NEAR((filterState.cov_-updateCov).norm(),0.0,1e-6); 132 | break; 133 | case 1: 134 | ASSERT_NEAR(dif.norm(),0.0,1e-10); 135 | ASSERT_NEAR((filterState.cov_-updateCov).norm(),0.0,1e-10); 136 | break; 137 | default: 138 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 139 | ASSERT_NEAR((filterState.cov_-updateCov).norm(),0.0,1e-6); 140 | } 141 | } 142 | 143 | // Test updateEKFWithOutlier 144 | TYPED_TEST(UpdateModelTest, updateEKFWithOutlier) { 145 | this->testUpdate_.meas_ = this->testUpdateMeas_; 146 | typename TestFixture::mtUpdateExample::mtFilterState filterState; 147 | filterState.cov_.setIdentity(); 148 | Eigen::MatrixXd H((int)(TestFixture::mtUpdateExample::mtInnovation::D_),(int)(TestFixture::mtUpdateExample::mtState::D_)); 149 | this->testUpdate_.jacState(H,this->testState_); 150 | Eigen::MatrixXd Hn((int)(TestFixture::mtUpdateExample::mtInnovation::D_),(int)(TestFixture::mtUpdateExample::mtNoise::D_)); 151 | this->testUpdate_.jacNoise(Hn,this->testState_); 152 | 153 | typename TestFixture::mtUpdateExample::mtInnovation y; 154 | this->testUpdate_.evalInnovationShort(y,this->testState_); 155 | typename TestFixture::mtUpdateExample::mtInnovation yIdentity; 156 | yIdentity.setIdentity(); 157 | typename TestFixture::mtUpdateExample::mtInnovation::mtDifVec innVector; 158 | 159 | typename TestFixture::mtUpdateExample::mtState stateUpdated; 160 | filterState.state_ = this->testState_; 161 | 162 | // Update 163 | MXD Py = H*filterState.cov_*H.transpose() + Hn*this->testUpdate_.updnoiP_*Hn.transpose(); 164 | y.boxMinus(yIdentity,innVector); 165 | Py.block(0,0,TestFixture::mtUpdateExample::mtInnovation::D_,3).setZero(); 166 | Py.block(0,0,3,TestFixture::mtUpdateExample::mtInnovation::D_).setZero(); 167 | Py.block(0,0,3,3).setIdentity(); 168 | H.block(0,0,3,TestFixture::mtUpdateExample::mtState::D_).setZero(); 169 | MXD Pyinv = Py.inverse(); 170 | 171 | // Kalman Update 172 | Eigen::Matrix K = filterState.cov_*H.transpose()*Pyinv; 173 | MXD updateCov = filterState.cov_ - K*Py*K.transpose(); 174 | typename TestFixture::mtUpdateExample::mtState::mtDifVec updateVec; 175 | updateVec = -K*innVector; 176 | filterState.state_.boxPlus(updateVec,stateUpdated); 177 | 178 | this->testUpdate_.outlierDetection_.setEnabledAll(true); 179 | this->testUpdate_.performUpdateEKF(filterState,this->testUpdateMeas_); 180 | typename TestFixture::mtUpdateExample::mtState::mtDifVec dif; 181 | filterState.state_.boxMinus(stateUpdated,dif); 182 | switch(TestFixture::id_){ 183 | case 0: 184 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 185 | ASSERT_NEAR((filterState.cov_-updateCov).norm(),0.0,1e-6); 186 | break; 187 | case 1: 188 | ASSERT_NEAR(dif.norm(),0.0,1e-10); 189 | ASSERT_NEAR((filterState.cov_-updateCov).norm(),0.0,1e-10); 190 | break; 191 | default: 192 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 193 | ASSERT_NEAR((filterState.cov_-updateCov).norm(),0.0,1e-6); 194 | } 195 | } 196 | 197 | // Test compareUpdate 198 | TYPED_TEST(UpdateModelTest, compareUpdate) { 199 | typename TestFixture::mtUpdateExample::mtFilterState filterState1; 200 | typename TestFixture::mtUpdateExample::mtFilterState filterState2; 201 | filterState1.cov_.setIdentity(); 202 | filterState1.cov_ = filterState1.cov_*0.000001; 203 | filterState2.cov_ = filterState1.cov_; 204 | filterState1.state_ = this->testState_; 205 | filterState2.state_ = this->testState_; 206 | this->testUpdate_.performUpdateEKF(filterState1,this->testUpdateMeas_); 207 | this->testUpdate_.performUpdateUKF(filterState2,this->testUpdateMeas_); 208 | typename TestFixture::mtUpdateExample::mtState::mtDifVec dif; 209 | filterState1.state_.boxMinus(filterState2.state_,dif); 210 | switch(TestFixture::id_){ 211 | case 0: 212 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 213 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-5); 214 | break; 215 | case 1: 216 | ASSERT_NEAR(dif.norm(),0.0,1e-10); 217 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-10); 218 | break; 219 | default: 220 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 221 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-5); 222 | } 223 | 224 | // Test with outlier detection 225 | filterState1.cov_.setIdentity(); 226 | filterState1.cov_ = filterState1.cov_*0.000001; 227 | filterState2.cov_ = filterState1.cov_; 228 | filterState1.state_ = this->testState_; 229 | filterState2.state_ = this->testState_; 230 | this->testUpdate_.outlierDetection_.setEnabledAll(true); 231 | this->testUpdate_.performUpdateEKF(filterState1,this->testUpdateMeas_); 232 | this->testUpdate_.performUpdateUKF(filterState2,this->testUpdateMeas_); 233 | filterState1.state_.boxMinus(filterState2.state_,dif); 234 | switch(TestFixture::id_){ 235 | case 0: 236 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 237 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-5); 238 | break; 239 | case 1: 240 | ASSERT_NEAR(dif.norm(),0.0,1e-10); 241 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-10); 242 | break; 243 | default: 244 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 245 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-5); 246 | } 247 | } 248 | 249 | // Test performPredictionAndUpdateEKF 250 | TYPED_TEST(UpdateModelTest, performPredictionAndUpdateEKF) { // Only tested without cross-correlation 251 | typename TestFixture::mtUpdateExample::mtFilterState filterState1; 252 | typename TestFixture::mtUpdateExample::mtFilterState filterState2; 253 | filterState1.cov_.setIdentity(); 254 | filterState1.cov_ = filterState1.cov_*0.000001; 255 | filterState2.cov_ = filterState1.cov_; 256 | filterState1.state_ = this->testState_; 257 | filterState2.state_ = this->testState_; 258 | this->testPrediction_.performPredictionEKF(filterState1,this->testPredictionMeas_,this->dt_); 259 | this->testUpdate_.performUpdateEKF(filterState1,this->testUpdateMeas_); 260 | this->testPrediction_.performPredictionEKF(filterState2,this->testPredictionMeas_,this->dt_); 261 | this->testPredictAndUpdate_.performUpdateEKF(filterState2,this->testUpdateMeas_); 262 | typename TestFixture::mtUpdateExample::mtState::mtDifVec dif; 263 | filterState1.state_.boxMinus(filterState2.state_,dif); 264 | switch(TestFixture::id_){ 265 | case 0: 266 | ASSERT_NEAR(dif.norm(),0.0,1e-8); 267 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-8); 268 | break; 269 | case 1: 270 | ASSERT_NEAR(dif.norm(),0.0,1e-10); 271 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-10); 272 | break; 273 | default: 274 | ASSERT_NEAR(dif.norm(),0.0,1e-8); 275 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-8); 276 | } 277 | 278 | // With outlier 279 | filterState1.cov_.setIdentity(); 280 | filterState1.cov_ = filterState1.cov_*0.000001; 281 | filterState2.cov_ = filterState1.cov_; 282 | filterState1.state_ = this->testState_; 283 | filterState2.state_ = this->testState_; 284 | this->testUpdate_.outlierDetection_.setEnabledAll(true); 285 | this->testPredictAndUpdate_.outlierDetection_.setEnabledAll(true); 286 | this->testPrediction_.performPredictionEKF(filterState1,this->testPredictionMeas_,this->dt_); 287 | this->testUpdate_.performUpdateEKF(filterState1,this->testUpdateMeas_); 288 | this->testPrediction_.performPredictionEKF(filterState2,this->testPredictionMeas_,this->dt_); 289 | this->testPredictAndUpdate_.performUpdateEKF(filterState2,this->testUpdateMeas_); 290 | filterState1.state_.boxMinus(filterState2.state_,dif); 291 | switch(TestFixture::id_){ 292 | case 0: 293 | ASSERT_NEAR(dif.norm(),0.0,1e-8); 294 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-8); 295 | break; 296 | case 1: 297 | ASSERT_NEAR(dif.norm(),0.0,1e-10); 298 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-10); 299 | break; 300 | default: 301 | ASSERT_NEAR(dif.norm(),0.0,1e-8); 302 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-8); 303 | } 304 | } 305 | 306 | // Test performPredictionAndUpdateUKF 307 | TYPED_TEST(UpdateModelTest, performPredictionAndUpdateUKF) { 308 | typename TestFixture::mtUpdateExample::mtFilterState filterState1; 309 | typename TestFixture::mtUpdateExample::mtFilterState filterState2; 310 | filterState1.cov_.setIdentity(); 311 | filterState1.cov_ = filterState1.cov_*0.000001; 312 | filterState2.cov_ = filterState1.cov_; 313 | filterState1.state_ = this->testState_; 314 | filterState2.state_ = this->testState_; 315 | this->testPrediction_.performPredictionUKF(filterState1,this->testPredictionMeas_,this->dt_); 316 | this->testUpdate_.performUpdateUKF(filterState1,this->testUpdateMeas_); 317 | this->testPrediction_.performPredictionUKF(filterState2,this->testPredictionMeas_,this->dt_); 318 | this->testPredictAndUpdate_.performUpdateUKF(filterState2,this->testUpdateMeas_); 319 | typename TestFixture::mtUpdateExample::mtState::mtDifVec dif; 320 | filterState1.state_.boxMinus(filterState2.state_,dif); 321 | switch(TestFixture::id_){ 322 | case 0: 323 | ASSERT_NEAR(dif.norm(),0.0,1e-4); // Increased difference comes because of different sigma points 324 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-6); 325 | break; 326 | case 1: 327 | ASSERT_NEAR(dif.norm(),0.0,2e-10); 328 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-10); 329 | break; 330 | default: 331 | ASSERT_NEAR(dif.norm(),0.0,1e-4); 332 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-6); 333 | } 334 | 335 | // With outlier 336 | filterState1.cov_.setIdentity(); 337 | filterState1.cov_ = filterState1.cov_*0.000001; 338 | filterState2.cov_ = filterState1.cov_; 339 | filterState1.state_ = this->testState_; 340 | filterState2.state_ = this->testState_; 341 | this->testUpdate_.outlierDetection_.setEnabledAll(true); 342 | this->testPredictAndUpdate_.outlierDetection_.setEnabledAll(true); 343 | this->testPrediction_.performPredictionUKF(filterState1,this->testPredictionMeas_,this->dt_); 344 | this->testUpdate_.performUpdateUKF(filterState1,this->testUpdateMeas_); 345 | this->testPrediction_.performPredictionUKF(filterState2,this->testPredictionMeas_,this->dt_); 346 | this->testPredictAndUpdate_.performUpdateUKF(filterState2,this->testUpdateMeas_); 347 | filterState1.state_.boxMinus(filterState2.state_,dif); 348 | switch(TestFixture::id_){ 349 | case 0: 350 | ASSERT_NEAR(dif.norm(),0.0,1e-4); // Increased difference comes because of different sigma points 351 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-6); 352 | break; 353 | case 1: 354 | ASSERT_NEAR(dif.norm(),0.0,2e-10); 355 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-10); 356 | break; 357 | default: 358 | ASSERT_NEAR(dif.norm(),0.0,1e-4); 359 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-6); 360 | } 361 | } 362 | 363 | // Test comparePredictAndUpdate (including correlated noise) 364 | TYPED_TEST(UpdateModelTest, comparePredictAndUpdate) { 365 | typename TestFixture::mtUpdateExample::mtFilterState filterState1; 366 | typename TestFixture::mtUpdateExample::mtFilterState filterState2; 367 | filterState1.cov_.setIdentity(); 368 | filterState1.cov_ = filterState1.cov_*0.0001; 369 | filterState2.cov_ = filterState1.cov_; 370 | filterState1.state_ = this->testState_; 371 | filterState2.state_ = this->testState_; 372 | this->testPrediction_.performPredictionEKF(filterState1,this->testPredictionMeas_,this->dt_); 373 | this->testPredictAndUpdate_.performUpdateEKF(filterState1,this->testUpdateMeas_); 374 | this->testPrediction_.performPredictionUKF(filterState2,this->testPredictionMeas_,this->dt_); 375 | this->testPredictAndUpdate_.performUpdateUKF(filterState2,this->testUpdateMeas_); 376 | typename TestFixture::mtUpdateExample::mtState::mtDifVec dif; 377 | filterState1.state_.boxMinus(filterState2.state_,dif); 378 | switch(TestFixture::id_){ 379 | case 0: 380 | ASSERT_NEAR(dif.norm(),0.0,2e-2); 381 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,2e-4); 382 | break; 383 | case 1: 384 | ASSERT_NEAR(dif.norm(),0.0,1e-9); 385 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-9); 386 | break; 387 | default: 388 | ASSERT_NEAR(dif.norm(),0.0,2e-2); 389 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,8e-5); 390 | } 391 | 392 | // Negativ Control (Based on above) 393 | filterState1.cov_.setIdentity(); 394 | filterState1.cov_ = filterState1.cov_*0.0001; 395 | filterState2.cov_ = filterState1.cov_; 396 | filterState1.state_ = this->testState_; 397 | filterState2.state_ = this->testState_; 398 | this->testPrediction_.performPredictionEKF(filterState1,this->testPredictionMeas_,this->dt_); 399 | this->testPredictAndUpdate_.performUpdateEKF(filterState1,this->testUpdateMeas_); 400 | this->testPredictAndUpdate_.preupdnoiP_.block(0,0,3,3) = M3D::Identity()*0.00009; 401 | this->testPrediction_.performPredictionUKF(filterState2,this->testPredictionMeas_,this->dt_); 402 | this->testPredictAndUpdate_.performUpdateUKF(filterState2,this->testUpdateMeas_); 403 | filterState1.state_.boxMinus(filterState2.state_,dif); 404 | switch(TestFixture::id_){ 405 | case 0: 406 | ASSERT_TRUE(dif.norm()>1e-1); 407 | ASSERT_TRUE((filterState1.cov_-filterState2.cov_).norm()>7e-5); // Bad discremination for nonlinear case 408 | break; 409 | case 1: 410 | ASSERT_TRUE(dif.norm()>1e-1); 411 | ASSERT_TRUE((filterState1.cov_-filterState2.cov_).norm()>1e-5); 412 | break; 413 | default: 414 | ASSERT_TRUE(dif.norm()>1e-1); 415 | ASSERT_TRUE((filterState1.cov_-filterState2.cov_).norm()>7e-5); 416 | } 417 | 418 | filterState1.cov_.setIdentity(); 419 | filterState1.cov_ = filterState1.cov_*0.0001; 420 | filterState2.cov_ = filterState1.cov_; 421 | filterState1.state_ = this->testState_; 422 | filterState2.state_ = this->testState_; 423 | this->testPrediction_.performPredictionEKF(filterState1,this->testPredictionMeas_,this->dt_); 424 | this->testPredictAndUpdate_.performUpdateEKF(filterState1,this->testUpdateMeas_); 425 | this->testPrediction_.performPredictionUKF(filterState2,this->testPredictionMeas_,this->dt_); 426 | this->testPredictAndUpdate_.performUpdateUKF(filterState2,this->testUpdateMeas_); 427 | filterState1.state_.boxMinus(filterState2.state_,dif); 428 | switch(TestFixture::id_){ 429 | case 0: 430 | ASSERT_NEAR(dif.norm(),0.0,2e-2); 431 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,2e-4); 432 | break; 433 | case 1: 434 | ASSERT_NEAR(dif.norm(),0.0,1e-9); 435 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-9); 436 | break; 437 | default: 438 | ASSERT_NEAR(dif.norm(),0.0,2e-2); 439 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,8e-5); 440 | } 441 | } 442 | 443 | // Test performUpdateLEKF1 (linearization point = prediction) 444 | TYPED_TEST(UpdateModelTest, performUpdateLEKF1) { 445 | typename TestFixture::mtUpdateExample::mtFilterState filterState1; 446 | typename TestFixture::mtUpdateExample::mtFilterState filterState2; 447 | // Linearization point 448 | typename TestFixture::mtUpdateExample::mtState linState; 449 | linState = this->testState_; 450 | 451 | filterState1.cov_.setIdentity(); 452 | filterState1.cov_ = filterState1.cov_*0.0001; 453 | filterState2.cov_ = filterState1.cov_; 454 | filterState1.state_ = this->testState_; 455 | filterState2.state_ = this->testState_; 456 | linState.boxMinus(filterState1.state_,filterState1.difVecLin_); 457 | this->testUpdate_.useSpecialLinearizationPoint_ = true; 458 | this->testUpdate_.performUpdateEKF(filterState1,this->testUpdateMeas_); 459 | this->testUpdate_.useSpecialLinearizationPoint_ = false; 460 | this->testUpdate_.performUpdateEKF(filterState2,this->testUpdateMeas_); 461 | typename TestFixture::mtUpdateExample::mtState::mtDifVec dif; 462 | filterState1.state_.boxMinus(filterState2.state_,dif); 463 | switch(TestFixture::id_){ 464 | case 0: 465 | ASSERT_NEAR(dif.norm(),0.0,2e-2); 466 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,8e-5); 467 | break; 468 | case 1: 469 | ASSERT_NEAR(dif.norm(),0.0,1e-9); 470 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-9); 471 | break; 472 | default: 473 | ASSERT_NEAR(dif.norm(),0.0,2e-2); 474 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,8e-5); 475 | } 476 | } 477 | 478 | // Test performUpdateLEKF2 (linearization point <> prediction, but for linear case still the same result) 479 | TYPED_TEST(UpdateModelTest, performUpdateLEKF2) { 480 | typename TestFixture::mtUpdateExample::mtFilterState filterState1; 481 | typename TestFixture::mtUpdateExample::mtFilterState filterState2; 482 | // Linearization point 483 | typename TestFixture::mtUpdateExample::mtState linState; 484 | linState = this->testState_; 485 | 486 | filterState1.cov_.setIdentity(); 487 | filterState1.cov_ = filterState1.cov_*0.0001; 488 | filterState2.cov_ = filterState1.cov_; 489 | filterState1.state_ = this->testState_; 490 | filterState2.state_ = this->testState_; 491 | linState.boxMinus(filterState1.state_,filterState1.difVecLin_); 492 | this->testUpdate_.useSpecialLinearizationPoint_ = true; 493 | this->testUpdate_.performUpdateEKF(filterState1,this->testUpdateMeas_); 494 | this->testUpdate_.useSpecialLinearizationPoint_ = false; 495 | this->testUpdate_.performUpdateEKF(filterState2,this->testUpdateMeas_); 496 | typename TestFixture::mtUpdateExample::mtState::mtDifVec dif; 497 | filterState1.state_.boxMinus(filterState2.state_,dif); 498 | switch(TestFixture::id_){ 499 | case 0: 500 | // No ASSERT 501 | break; 502 | case 1: 503 | ASSERT_NEAR(dif.norm(),0.0,1e-10); 504 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-10); 505 | break; 506 | default: 507 | // No ASSERT 508 | break; 509 | } 510 | } 511 | 512 | // Test performUpdateLEKF3 (linearization point <> prediction, general) 513 | TYPED_TEST(UpdateModelTest, performUpdateLEKF3) { 514 | this->testUpdate_.meas_ = this->testUpdateMeas_; 515 | // Linearization point 516 | typename TestFixture::mtUpdateExample::mtFilterState filterState; 517 | filterState.state_ = this->testState_; 518 | typename TestFixture::mtUpdateExample::mtState linState; 519 | unsigned int s = 2; 520 | linState.setRandom(s); 521 | 522 | filterState.cov_.setIdentity(); 523 | Eigen::MatrixXd H((int)(TestFixture::mtUpdateExample::mtInnovation::D_),(int)(TestFixture::mtUpdateExample::mtState::D_)); 524 | this->testUpdate_.jacState(H,linState); 525 | Eigen::MatrixXd Hlin((int)(TestFixture::mtUpdateExample::mtInnovation::D_),(int)(TestFixture::mtUpdateExample::mtState::D_)); 526 | if(this->testUpdate_.useImprovedJacobian_){ 527 | MXD boxMinusJac((int)TestFixture::mtState::D_,(int)TestFixture::mtState::D_); 528 | filterState.state_.boxMinusJac(linState,boxMinusJac); 529 | Hlin = H*boxMinusJac; 530 | } else { 531 | Hlin = H; 532 | } 533 | Eigen::MatrixXd Hn((int)(TestFixture::mtUpdateExample::mtInnovation::D_),(int)(TestFixture::mtUpdateExample::mtNoise::D_)); 534 | this->testUpdate_.jacNoise(Hn,linState); 535 | 536 | typename TestFixture::mtUpdateExample::mtInnovation y; 537 | this->testUpdate_.evalInnovationShort(y,linState); 538 | typename TestFixture::mtUpdateExample::mtInnovation yIdentity; 539 | yIdentity.setIdentity(); 540 | typename TestFixture::mtUpdateExample::mtInnovation::mtDifVec innVector; 541 | 542 | typename TestFixture::mtUpdateExample::mtState stateUpdated; 543 | 544 | // Update 545 | MXD Py = Hlin*filterState.cov_*Hlin.transpose() + Hn*this->testUpdate_.updnoiP_*Hn.transpose(); 546 | y.boxMinus(yIdentity,innVector); 547 | MXD Pyinv = Py.inverse(); 548 | 549 | // Kalman Update 550 | Eigen::Matrix K = filterState.cov_*Hlin.transpose()*Pyinv; 551 | MXD updateCov = filterState.cov_ - K*Py*K.transpose(); 552 | typename TestFixture::mtUpdateExample::mtState::mtDifVec updateVec; 553 | typename TestFixture::mtUpdateExample::mtState::mtDifVec difVecLin; 554 | linState.boxMinus(filterState.state_,difVecLin); 555 | updateVec = -K*(innVector-H*difVecLin); 556 | filterState.state_.boxPlus(updateVec,stateUpdated); 557 | 558 | linState.boxMinus(filterState.state_,filterState.difVecLin_); 559 | this->testUpdate_.useSpecialLinearizationPoint_ = true; 560 | this->testUpdate_.performUpdateEKF(filterState,this->testUpdateMeas_); 561 | this->testUpdate_.useSpecialLinearizationPoint_ = false; 562 | typename TestFixture::mtUpdateExample::mtState::mtDifVec dif; 563 | filterState.state_.boxMinus(stateUpdated,dif); 564 | switch(TestFixture::id_){ 565 | case 0: 566 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 567 | ASSERT_NEAR((filterState.cov_-updateCov).norm(),0.0,1e-6); 568 | break; 569 | case 1: 570 | ASSERT_NEAR(dif.norm(),0.0,1e-10); 571 | ASSERT_NEAR((filterState.cov_-updateCov).norm(),0.0,1e-10); 572 | break; 573 | default: 574 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 575 | ASSERT_NEAR((filterState.cov_-updateCov).norm(),0.0,1e-6); 576 | break; 577 | } 578 | } 579 | 580 | // Test performUpdateIEKF1 (for linear case still the same result) 581 | TYPED_TEST(UpdateModelTest, performUpdateIEKF1) { 582 | typename TestFixture::mtUpdateExample::mtFilterState filterState1; 583 | typename TestFixture::mtUpdateExample::mtFilterState filterState2; 584 | filterState1.cov_.setIdentity(); 585 | filterState1.cov_ = filterState1.cov_*0.0001; 586 | filterState2.cov_ = filterState1.cov_; 587 | filterState1.state_ = this->testState_; 588 | filterState2.state_ = this->testState_; 589 | this->testUpdate_.performUpdateIEKF(filterState1,this->testUpdateMeas_); 590 | this->testUpdate_.performUpdateEKF(filterState2,this->testUpdateMeas_); 591 | typename TestFixture::mtUpdateExample::mtState::mtDifVec dif; 592 | filterState1.state_.boxMinus(filterState2.state_,dif); 593 | switch(TestFixture::id_){ 594 | case 0: 595 | // No ASSERT 596 | break; 597 | case 1: 598 | ASSERT_NEAR(dif.norm(),0.0,1e-10); 599 | ASSERT_NEAR((filterState1.cov_-filterState2.cov_).norm(),0.0,1e-10); 600 | break; 601 | default: 602 | // No ASSERT 603 | break; 604 | } 605 | } 606 | 607 | // Test performUpdateIEKF2 (general test) 608 | TYPED_TEST(UpdateModelTest, performUpdateIEKF2) { 609 | this->testUpdate_.meas_ = this->testUpdateMeas_; 610 | typename TestFixture::mtUpdateExample::mtFilterState filterState; 611 | // Linearization point 612 | typename TestFixture::mtUpdateExample::mtState linState; 613 | linState = this->testState_; 614 | 615 | filterState.cov_.setIdentity(); 616 | Eigen::MatrixXd H((int)(TestFixture::mtUpdateExample::mtInnovation::D_),(int)(TestFixture::mtUpdateExample::mtState::D_)); 617 | Eigen::MatrixXd Hn((int)(TestFixture::mtUpdateExample::mtInnovation::D_),(int)(TestFixture::mtUpdateExample::mtNoise::D_)); 618 | 619 | typename TestFixture::mtUpdateExample::mtInnovation y; 620 | typename TestFixture::mtUpdateExample::mtInnovation yIdentity; 621 | yIdentity.setIdentity(); 622 | typename TestFixture::mtUpdateExample::mtInnovation::mtDifVec innVector; 623 | 624 | typename TestFixture::mtUpdateExample::mtState stateUpdated; 625 | filterState.state_ = this->testState_; 626 | 627 | MXD Py; 628 | MXD Pyinv; 629 | Eigen::Matrix K; 630 | typename TestFixture::mtUpdateExample::mtState::mtDifVec updateVec; 631 | typename TestFixture::mtUpdateExample::mtState::mtDifVec difVecLin; 632 | 633 | double updateVecNorm = this->testUpdate_.updateVecNormTermination_; 634 | for(unsigned int i=0;itestUpdate_.maxNumIteration_ & updateVecNorm>=this->testUpdate_.updateVecNormTermination_;i++){ 635 | this->testUpdate_.jacState(H,linState); 636 | this->testUpdate_.jacNoise(Hn,linState); 637 | 638 | this->testUpdate_.evalInnovationShort(y,linState); 639 | 640 | // Update 641 | Py = H*filterState.cov_*H.transpose() + Hn*this->testUpdate_.updnoiP_*Hn.transpose(); 642 | y.boxMinus(yIdentity,innVector); 643 | Pyinv = Py.inverse(); 644 | 645 | // Kalman Update 646 | K = filterState.cov_*H.transpose()*Pyinv; 647 | filterState.state_.boxMinus(linState,difVecLin); 648 | updateVec = -K*(innVector+H*difVecLin)+difVecLin; 649 | linState.boxPlus(updateVec,linState); 650 | updateVecNorm = updateVec.norm(); 651 | } 652 | stateUpdated = linState; 653 | MXD updateCov = filterState.cov_ - K*Py*K.transpose(); 654 | 655 | this->testUpdate_.performUpdateIEKF(filterState,this->testUpdateMeas_); 656 | typename TestFixture::mtUpdateExample::mtState::mtDifVec dif; 657 | filterState.state_.boxMinus(stateUpdated,dif); 658 | switch(TestFixture::id_){ 659 | case 0: 660 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 661 | ASSERT_NEAR((filterState.cov_-updateCov).norm(),0.0,1e-6); 662 | break; 663 | case 1: 664 | ASSERT_NEAR(dif.norm(),0.0,1e-10); 665 | ASSERT_NEAR((filterState.cov_-updateCov).norm(),0.0,1e-10); 666 | break; 667 | default: 668 | ASSERT_NEAR(dif.norm(),0.0,1e-6); 669 | ASSERT_NEAR((filterState.cov_-updateCov).norm(),0.0,1e-6); 670 | break; 671 | } 672 | } 673 | 674 | int main(int argc, char **argv) { 675 | ::testing::InitGoogleTest(&argc, argv); 676 | return RUN_ALL_TESTS(); 677 | } 678 | --------------------------------------------------------------------------------