00001 /************************************************************************ 00002 * Copyright (C) 2012 Eindhoven University of Technology (TU/e). * 00003 * All rights reserved. * 00004 ************************************************************************ 00005 * Redistribution and use in source and binary forms, with or without * 00006 * modification, are permitted provided that the following conditions * 00007 * are met: * 00008 * * 00009 * 1. Redistributions of source code must retain the above * 00010 * copyright notice, this list of conditions and the following * 00011 * disclaimer. * 00012 * * 00013 * 2. Redistributions in binary form must reproduce the above * 00014 * copyright notice, this list of conditions and the following * 00015 * disclaimer in the documentation and/or other materials * 00016 * provided with the distribution. * 00017 * * 00018 * THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR * 00019 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * 00020 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 00021 * ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE * 00022 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * 00023 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT * 00024 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; * 00025 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 00026 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 00027 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE * 00028 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH * 00029 * DAMAGE. * 00030 * * 00031 * The views and conclusions contained in the software and * 00032 * documentation are those of the authors and should not be * 00033 * interpreted as representing official policies, either expressed or * 00034 * implied, of TU/e. * 00035 ************************************************************************/ 00036 00037 #ifndef SE_POSITION_FILTER_H_ 00038 #define SE_POSITION_FILTER_H_ 00039 00040 #include "wire/core/IStateEstimator.h" 00041 #include "problib/conversions.h" 00042 00043 class KalmanFilter; 00044 00052 class PositionFilter : public mhf::IStateEstimator { 00053 00054 public: 00055 00056 PositionFilter(); 00057 00058 PositionFilter(const PositionFilter& orig); 00059 00060 virtual PositionFilter* clone() const; 00061 00062 virtual ~PositionFilter(); 00063 00068 virtual void propagate(const mhf::Time& time); 00069 00075 void update(const pbl::PDF& z, const mhf::Time& time); 00076 00080 virtual void reset(); 00081 00086 const pbl::PDF& getValue() const; 00087 00088 void setValue(const pbl::PDF& pdf); 00089 00096 bool setParameter(const std::string& param, bool b); 00097 00104 bool setParameter(const std::string& param, double v); 00105 00106 protected: 00107 00108 mhf::Time t_last_update_; 00109 00110 mhf::Time t_last_propagation_; 00111 00112 KalmanFilter* kalman_filter_; 00113 00114 pbl::Gaussian* fixed_pdf_; 00115 00116 // ********* filter parameters ********* 00117 00118 double max_acceleration_; 00119 00120 double fixed_pdf_cov_; 00121 00122 double kalman_timeout_; 00123 00124 }; 00125 00126 #endif