Bayesian Filtering Library  Generated from SVN r
extendedkalmanfilter.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 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 __EXTENDED_KALMAN_FILTER__
22 #define __EXTENDED_KALMAN_FILTER__
23 
24 #include "kalmanfilter.h"
25 #include "../pdf/conditionalpdf.h"
26 #include "../pdf/gaussian.h"
27 # include <map>
28 
29 namespace BFL
30 {
31 
44 {
45 public:
51 
53  virtual ~ExtendedKalmanFilter();
54 
56  // For realtime use, this function should be called before calling measUpdate
57  /* @param vector containing the dimension of the measurement models which are
58  going to be used
59  */
60  void AllocateMeasModelExt( const vector<unsigned int>& meas_dimensions);
61 
63  // For realtime use, this function should be called before calling measUpdate
64  /* @param dimension of the measurement models which is
65  going to be used
66  */
67  void AllocateMeasModelExt( const unsigned int& meas_dimensions);
68 
69  // Get NIS value
71 
72 private:
73  struct MeasUpdateVariablesExt
74  {
75  SymmetricMatrix _R;
76  Matrix _H;
77  ColumnVector _Z;
78  MeasUpdateVariablesExt() {};
79  MeasUpdateVariablesExt(unsigned int meas_dimension, unsigned int state_dimension):
80  _R(meas_dimension)
81  , _H(meas_dimension,state_dimension)
82  , _Z(meas_dimension)
83 {};
84  }; //struct
85 
86  protected:
87  virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
92  // variables to avoid allocation on the heap
93  ColumnVector _x;
94  ColumnVector _J;
95  Matrix _F;
96  SymmetricMatrix _Q;
97  std::map<unsigned int, MeasUpdateVariablesExt> _mapMeasUpdateVariablesExt;
98  std::map<unsigned int, MeasUpdateVariablesExt>::iterator _mapMeasUpdateVariablesExt_it;
99 
100 
101 }; // class
102 
103 } // End namespace BFL
104 
105 #endif // __EXTENDED_KALMAN_FILTER__
Class representing Gaussian (or normal density)
Definition: gaussian.h:27
virtual ~ExtendedKalmanFilter()
Destructor.
Wrapper class for ColumnVectors (Boost implementation)
Definition: asirfilter.h:41
virtual void SysUpdate(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)
System Update.
Class representing the family of all Kalman Filters (EKF, IEKF, ...)
Definition: kalmanfilter.h:49
ExtendedKalmanFilter(Gaussian *prior)
virtual void MeasUpdate(MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
Measurement Update (overloaded)
void AllocateMeasModelExt(const vector< unsigned int > &meas_dimensions)
Function to allocate memory needed during the measurement update,.