$search
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_KALMANFILTER_H_ 00038 #define SE_KALMANFILTER_H_ 00039 00040 #include "problib/pdfs/Gaussian.h" 00041 00046 class KalmanFilter { 00047 public: 00048 00053 KalmanFilter(int dim); 00054 00058 KalmanFilter(const KalmanFilter& orig); 00059 00063 virtual KalmanFilter* clone() const; 00064 00068 virtual ~KalmanFilter(); 00069 00076 virtual void init(const pbl::Gaussian& G); 00077 00082 void update(const pbl::Gaussian& z); 00083 00089 virtual void propagate(const double& dt); 00090 00097 double getLikelihood(const pbl::Gaussian& z) const; 00098 00103 const pbl::Gaussian& getGaussian() const; 00104 00109 const pbl::Vector& getState() const; 00110 00115 const pbl::Matrix& getStateCovariance() const; 00116 00124 void setMaxAcceleration(double a_max); 00125 00126 protected: 00127 00129 int meas_dim_; 00130 00132 int state_dim_; 00133 00135 pbl::Gaussian G_; 00136 00138 pbl::Gaussian G_small_; 00139 00141 pbl::Matrix H_; 00142 00144 double a_max_; 00145 }; 00146 00147 #endif /* KALMANTRACKER_H_ */