kalmanfilter.h
Go to the documentation of this file.
1 // $Id$
2 // Copyright (C) 2002 Klaas Gadeyne <first dot last at gmail dot com>
3 // Wim Meeussen <wim dot meeussen at mech dot kuleuven dot ac dot be>
4 // Tinne De Laet <tinne dot delaet at mech dot kuleuven dot be>
5 //
6 // This program is free software; you can redistribute it and/or modify
7 // it under the terms of the GNU Lesser General Public License as published by
8 // the Free Software Foundation; either version 2.1 of the License, or
9 // (at your option) any later version.
10 //
11 // This program is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 // GNU Lesser General Public License for more details.
15 //
16 // You should have received a copy of the GNU Lesser General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
19 //
20 
21 #ifndef __KALMAN_FILTER__
22 #define __KALMAN_FILTER__
23 
24 #include "filter.h"
25 #include "../pdf/gaussian.h"
26 #include "../model/analyticmeasurementmodel_gaussianuncertainty.h"
27 #include "../model/analyticsystemmodel_gaussianuncertainty.h"
28 # include <map>
29 
30 namespace BFL
31 {
32 
34 
49 class KalmanFilter : public Filter<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>
50 {
51 public:
53 
56  KalmanFilter(Gaussian* prior);
57 
59  virtual ~KalmanFilter();
60 
61  // implement virtual function
62  virtual Gaussian* PostGet();
63 
65  // For realtime use, this function should be called before calling measUpdate
66  /* @param vector containing the dimension of the measurement models which are
67  going to be used
68  */
69  void AllocateMeasModel( const vector<unsigned int>& meas_dimensions);
70 
72  // For realtime use, this function should be called before calling measUpdate
73  /* @param dimension of the measurement models which is
74  going to be used
75  */
76  void AllocateMeasModel( const unsigned int& meas_dimensions);
77 
78 private:
80  {
81  Matrix _S_Matrix;
82  Matrix _K;
83  ColumnVector _innov;
84  Matrix _postHT;
86  MeasUpdateVariables(unsigned int meas_dimension, unsigned int state_dimension):
87  _S_Matrix(meas_dimension,meas_dimension)
88  , _K(state_dimension,meas_dimension)
89  , _innov(meas_dimension)
90  , _postHT(state_dimension,meas_dimension)
91 {};
92  }; //struct
93 
94 protected:
95  // variables to avoid allocation during update calls
96  ColumnVector _Mu_new;
97  SymmetricMatrix _Sigma_new;
98  Matrix _Sigma_temp;
100  Matrix _SMatrix;
101  Matrix _K;
102  std::map<unsigned int, MeasUpdateVariables> _mapMeasUpdateVariables;
103  std::map<unsigned int, MeasUpdateVariables>::iterator _mapMeasUpdateVariables_it;
104 
105 
111 
113  void PostSigmaSet( const MatrixWrapper::SymmetricMatrix& s);
114 
116  void PostMuSet( const MatrixWrapper::ColumnVector& c);
117 
122  void CalculateSysUpdate(const MatrixWrapper::ColumnVector& J, const MatrixWrapper::Matrix& F, const MatrixWrapper::SymmetricMatrix& Q);
123 
130  void CalculateMeasUpdate(const MatrixWrapper::ColumnVector& z, const MatrixWrapper::ColumnVector& Z, const MatrixWrapper::Matrix& H, const MatrixWrapper::SymmetricMatrix& R);
131 
133 
138  virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
139  const MatrixWrapper::ColumnVector& u) = 0;
140 
142 
154  const MatrixWrapper::ColumnVector& z,
155  const MatrixWrapper::ColumnVector& s) = 0;
156 
157  virtual bool UpdateInternal(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
158  const MatrixWrapper::ColumnVector& u,
160  const MatrixWrapper::ColumnVector& z,
161  const MatrixWrapper::ColumnVector& s);
162 
163 }; // class
164 
165 
166 
167 } // End namespace BFL
168 
169 #endif // __KALMAN_FILTER__
Abstract class representing an interface for Bayesian Filters.
Definition: filter.h:77
void PostMuSet(const MatrixWrapper::ColumnVector &c)
Set expected value of posterior estimate.
Matrix _Sigma_temp_par
Definition: kalmanfilter.h:99
virtual ~KalmanFilter()
Destructor.
std::map< unsigned int, MeasUpdateVariables >::iterator _mapMeasUpdateVariables_it
Definition: kalmanfilter.h:103
Class representing Gaussian (or normal density)
Definition: gaussian.h:27
void PostSigmaSet(const MatrixWrapper::SymmetricMatrix &s)
Set covariance of posterior estimate.
KalmanFilter(Gaussian *prior)
Constructor.
virtual bool UpdateInternal(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u, MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
Actual implementation of Update, varies along filters.
MeasUpdateVariables(unsigned int meas_dimension, unsigned int state_dimension)
Definition: kalmanfilter.h:86
Class representing the family of all Kalman Filters (EKF, IEKF, ...)
Definition: kalmanfilter.h:49
virtual Gaussian * PostGet()
Get Posterior density.
void CalculateSysUpdate(const MatrixWrapper::ColumnVector &J, const MatrixWrapper::Matrix &F, const MatrixWrapper::SymmetricMatrix &Q)
std::map< unsigned int, MeasUpdateVariables > _mapMeasUpdateVariables
Definition: kalmanfilter.h:102
virtual void SysUpdate(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)=0
System Update.
ColumnVector _Mu_new
Definition: kalmanfilter.h:96
void CalculateMeasUpdate(const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &Z, const MatrixWrapper::Matrix &H, const MatrixWrapper::SymmetricMatrix &R)
virtual void MeasUpdate(MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)=0
Measurement Update (overloaded)
void AllocateMeasModel(const vector< unsigned int > &meas_dimensions)
Function to allocate memory needed during the measurement update,.
SymmetricMatrix _Sigma_new
Definition: kalmanfilter.h:97


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Mon Feb 28 2022 21:56:33