Bayesian Filtering Library  Generated from SVN r
iteratedextendedkalmanfilter.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 __ITERATED_EXTENDED_KALMAN_FILTER__
22 #define __ITERATED_EXTENDED_KALMAN_FILTER__
23 
24 #include "kalmanfilter.h"
25 #include "../pdf/conditionalpdf.h"
26 #include "../pdf/gaussian.h"
27 #include "innovationCheck.h"
28 # include <map>
29 
30 namespace BFL
31 {
41  {
42  protected:
43  virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
48 
49  public:
56  IteratedExtendedKalmanFilter(Gaussian* prior, unsigned int nr_it=1, InnovationCheck* innov = NULL);
57 
60 
62  // For realtime use, this function should be called before calling measUpdate
63  /* @param vector containing the dimension of the measurement models which are
64  going to be used
65  */
66  void AllocateMeasModelIExt( const vector<unsigned int>& meas_dimensions);
67 
69  // For realtime use, this function should be called before calling measUpdate
70  /* @param dimension of the measurement models which is
71  going to be used
72  */
73  void AllocateMeasModelIExt( const unsigned int& meas_dimensions);
74 
75  private:
77  unsigned int _nr_iterations;
79  InnovationCheck* _innovationChecker;
80 
81  struct MeasUpdateVariablesIExt
82  {
83  SymmetricMatrix _R_i;
84  Matrix _K_i;
85  Matrix _H_i;
86  ColumnVector _Z_i;
87  MeasUpdateVariablesIExt() {};
88  MeasUpdateVariablesIExt(unsigned int meas_dimension, unsigned int state_dimension):
89  _R_i(meas_dimension)
90  , _K_i(state_dimension,meas_dimension)
91  , _H_i(meas_dimension,state_dimension)
92  , _Z_i(meas_dimension)
93  {};
94  }; //struct
95 
96  // variables to avoid allocation on the heap
97  ColumnVector _x;
98  ColumnVector _x_i;
99  ColumnVector _x_i_prev;
100  ColumnVector _J;
101  ColumnVector _innovation;
102  Matrix _F;
103  SymmetricMatrix _Q;
104  SymmetricMatrix _P_Matrix;
105  Matrix _S_i;
106  std::map<unsigned int, MeasUpdateVariablesIExt> _mapMeasUpdateVariablesIExt;
107  std::map<unsigned int, MeasUpdateVariablesIExt>::iterator _mapMeasUpdateVariablesIExt_it;
108  }; // class
109 
110 } // End namespace BFL
111 
112 #endif // __ITERATED_EXTENDED_KALMAN_FILTER__
113 
BFL::IteratedExtendedKalmanFilter::~IteratedExtendedKalmanFilter
virtual ~IteratedExtendedKalmanFilter()
Destructor.
BFL::Gaussian
Class representing Gaussian (or normal density)
Definition: gaussian.h:27
BFL::IteratedExtendedKalmanFilter::SysUpdate
virtual void SysUpdate(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)
System Update.
MatrixWrapper::ColumnVector
Wrapper class for ColumnVectors (Boost implementation)
Definition: vector_BOOST.h:41
BFL::IteratedExtendedKalmanFilter::IteratedExtendedKalmanFilter
IteratedExtendedKalmanFilter(Gaussian *prior, unsigned int nr_it=1, InnovationCheck *innov=NULL)
BFL::IteratedExtendedKalmanFilter
Definition: iteratedextendedkalmanfilter.h:40
BFL::MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector >
BFL::InnovationCheck
Class implementing an innovationCheck used in IEKF.
Definition: innovationCheck.h:33
BFL::SystemModel< MatrixWrapper::ColumnVector >
BFL::IteratedExtendedKalmanFilter::AllocateMeasModelIExt
void AllocateMeasModelIExt(const vector< unsigned int > &meas_dimensions)
Function to allocate memory needed during the measurement update,.
BFL::IteratedExtendedKalmanFilter::MeasUpdate
virtual void MeasUpdate(MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
Measurement Update (overloaded)
BFL::KalmanFilter
Class representing the family of all Kalman Filters (EKF, IEKF, ...)
Definition: kalmanfilter.h:49