Bayesian Filtering Library  Generated from SVN r
kalmanfilter.h
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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 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 
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:
79  struct MeasUpdateVariables
80  {
81  Matrix _S_Matrix;
82  Matrix _K;
83  ColumnVector _innov;
84  Matrix _postHT;
85  MeasUpdateVariables() {};
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;
99  Matrix _Sigma_temp_par;
100  Matrix _SMatrix;
101  Matrix _K;
102  double _Nis;
103  std::map<unsigned int, MeasUpdateVariables> _mapMeasUpdateVariables;
104  std::map<unsigned int, MeasUpdateVariables>::iterator _mapMeasUpdateVariables_it;
105 
106 
112 
114  void PostSigmaSet( const MatrixWrapper::SymmetricMatrix& s);
115 
117  void PostMuSet( const MatrixWrapper::ColumnVector& c);
118 
123  void CalculateSysUpdate(const MatrixWrapper::ColumnVector& J, const MatrixWrapper::Matrix& F, const MatrixWrapper::SymmetricMatrix& Q);
124 
131  void CalculateMeasUpdate(const MatrixWrapper::ColumnVector& z, const MatrixWrapper::ColumnVector& Z, const MatrixWrapper::Matrix& H, const MatrixWrapper::SymmetricMatrix& R);
132 
134 
140  const MatrixWrapper::ColumnVector& u) = 0;
141 
143 
155  const MatrixWrapper::ColumnVector& z,
156  const MatrixWrapper::ColumnVector& s) = 0;
157 
159  const MatrixWrapper::ColumnVector& u,
161  const MatrixWrapper::ColumnVector& z,
162  const MatrixWrapper::ColumnVector& s);
163 
164  // Calculate Normalised Innovation Squared (NIS) value
165  void CalculateNis(const MatrixWrapper::ColumnVector& z, const MatrixWrapper::ColumnVector& Z, const MatrixWrapper::Matrix& H, const MatrixWrapper::SymmetricMatrix& R);
166 
167 }; // class
168 
169 
170 
171 } // End namespace BFL
172 
173 #endif // __KALMAN_FILTER__
Abstract class representing an interface for Bayesian Filters.
Definition: filter.h:78
Class representing Gaussian (or normal density)
Definition: gaussian.h:28
Class representing the family of all Kalman Filters (EKF, IEKF, ...)
Definition: kalmanfilter.h:50
void CalculateMeasUpdate(const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &Z, const MatrixWrapper::Matrix &H, const MatrixWrapper::SymmetricMatrix &R)
virtual void SysUpdate(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)=0
System Update.
void CalculateSysUpdate(const MatrixWrapper::ColumnVector &J, const MatrixWrapper::Matrix &F, const MatrixWrapper::SymmetricMatrix &Q)
virtual Gaussian * PostGet()
Get Posterior density.
void AllocateMeasModel(const vector< unsigned int > &meas_dimensions)
Function to allocate memory needed during the measurement update,.
void AllocateMeasModel(const unsigned int &meas_dimensions)
Function to allocate memory needed during the measurement update.
void PostSigmaSet(const MatrixWrapper::SymmetricMatrix &s)
Set covariance of posterior estimate.
virtual ~KalmanFilter()
Destructor.
KalmanFilter(Gaussian *prior)
Constructor.
void PostMuSet(const MatrixWrapper::ColumnVector &c)
Set expected value of posterior estimate.
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.
virtual void MeasUpdate(MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)=0
Measurement Update (overloaded)